
//
// Created by sukai on 4/1/23.
//

#include "../currencyinclude/global_planner.h"
using namespace contnav_global_planner;
namespace contnav_global_planner {
    global_planner::global_planner(tf2_ros::Buffer& tf,ros::NodeHandle &nh): tf_(tf),nh_(nh), state_machine_(nh)

                                                         //as_(NULL),
                                                         //点到点     GotoCtrl = actionlib::SimpleActionClient<mbf_msgs::MoveBaseAction>;//点到点
                                                         //goto_ctrl_(new GotoCtrl("move_base_flex/move_base")),
                                                          //给路径直接走   ExeCtrl = actionlib::SimpleActionClient<mbf_msgs::ExePathAction>;
                                                         //exe_ctrl_(new ExeCtrl("move_base_flex/exe_path")),
                                                         //ac_("/move_base_flex/exe_path", true)//ExeCtrl

                                                         {





        //状态机
        state_machine_.add_state("navigate_to_goalpath", std::bind(&global_planner::navigate_to_goalpath, this, std::placeholders::_1));//加入状态 navigate_to_goalPath 导航状态
        state_machine_.add_state("navigate_to_goalpathOverturn", std::bind(&global_planner::navigate_to_goalpathOverturn, this, std::placeholders::_1));//加入状态 navigate_to_goalPathAgain 导航状态重复发布导航

        //初始化给一个控制
        geometry_msgs::PoseStamped start_pose,target_pose,target_pose_default;
        std::vector<geometry_msgs::PoseStamped> plan_bendi;
        state_machine_.set_state("idle", {start_pose,target_pose,target_pose_default,std::string("EMPTY"),plan_bendi});//闲置状态,初始化给一个控制
//--------------------------------------------------------------------------------------------------
//idle 闲置状态

//1.选择点位设置全局路径 init_global_waypoints

//2.计算路径及各路径的链接点  init_global_path


//3.开始路径导航 navigate_to_goalpath


//4.导航过程中的状态机状态显示有障碍物的处理


//5.有障碍物是否需要等待时间，1.超时后原路返回 2.超时后点到点导航绕过障碍物


//6.导航成功时的状态



//---------------------------------------------------------------------------------------------------
    }

    global_planner::~global_planner() {}


//日志级别 日志内容
    bool global_planner::saveLogFormat(string logLevel, string className, int classLine, char *format, ...) {

        string logcontent = "";
        va_list arg_ptr;
        va_start(arg_ptr, format);
        int len = vsnprintf(NULL, 0, format, arg_ptr);
        va_end(arg_ptr);
        if (len > 0) {
            char *content = new char[len + 1];
            va_start(arg_ptr, format);
            vsnprintf(content, len + 1, format, arg_ptr);
            va_end(arg_ptr);
            content[len] = 0;
            logcontent = content;
            delete[] content; //动态的数组需要删除
            // m_len += len;//获取到当前日志的长度 累计起来
        }
        cout << "logcontent: " << logcontent << endl;

       // contnavLoggerClient.waitForExistence();//可以不存在log服务节点
        contnav_srvs::ContnavLogger contnavLoggerService;
        contnavLoggerService.request.request_type = ContnavLoggerType::savelog;// 保存日志   ContnavLoggerType::savelog:保存日志||   ContnavLoggerType::deleteAlllog:删除所有日志文件  ||  ContnavLoggerType::showlogSize 查看日志大小
        contnavLoggerService.request.logFileName = logFileName;// 日志文件名称
        contnavLoggerService.request.className = className;// 代码file名称 [注意： 确保每个服务节点只使用一个日志文件名,日志服务节点是根据文件名来创建对应的日志对象，请确保一个服务对应一个日志文件名称]
        contnavLoggerService.request.classLine = classLine;// 代码行数据
        contnavLoggerService.request.logLevel = logLevel;//日志类型,默认 debug
        contnavLoggerService.request.logcontent = logcontent;//日志内容

        //发送seervice
        contnavLoggerClient.call(contnavLoggerService);

        if (contnavLoggerService.response.result != "ok") {

            cout << "close ros  test.cppcontnavLoggerService.response.message: " << contnavLoggerService.response.message
                 << endl;
            return false;
        }
        return true;


    }

//sql 查询字典数据
//    void global_planner::queryAllFromTABLE_DICTIONARES() {
//        try {
//
//            std::cout << "==1================global_planner 查询字典数据===================" << std::endl;
//            //查询字典数据
//            SqlOperationClient.waitForExistence();
//            contnav_srvs::TABLE_CONTORLLER tableContorllerService;
//            tableContorllerService.request.request_type = TabeOperationType::selectSQLTABL;//selectSQLTABL：查询数据操作
//            tableContorllerService.request.request_where_dictionariesVector.request_type = TabeName::TABLE_DICTIONARES;//要操作的数据库表
//            tableContorllerService.request.isReturn_result_sqlStr = "true";//需要返回sql语句
//
//            /**
//               * 打开注释为按条件查询
//           //1. where 条件
//           whereSql("","Id","=","1",tableContorllerService);
//          */
//            //发送seervice
//            SqlOperationClient.call(tableContorllerService);
//
//            if (tableContorllerService.response.result != "ok")
//                return;
//
//            for (int l = 0; l < tableContorllerService.response.result_table_dictionares.size(); ++l) {
//                contnav_msgs::TABLE_DICTIONARES table_dictionares = tableContorllerService.response.result_table_dictionares[l];
//                dictionariesMap_[table_dictionares.Key_i] = table_dictionares.Value;
//                cout << "Id: " << table_dictionares.Id << ", ";
//                cout << "TargetName: " << table_dictionares.TargetName << ", ";
//                cout << "Key_i: " << table_dictionares.Key_i << ", ";
//                cout << "Value: " << table_dictionares.Value << ", ";
//                cout << "Status: " << table_dictionares.Status << "; " << endl;
//
//            }
//            /*
//            //遍历map,查看数据
//            map<string, string>::iterator iter;
//            for(iter = dictionariesMap_.begin(); iter != dictionariesMap_.end(); iter++) {
//                cout << iter->first << " : " << iter->second << endl;
//            }
//           */
//        } catch (exception e) {
//            cout << e.what() << endl;
//        }
//    }

//sql where 条件
//    void global_planner::whereSql(std::string Condition, std::string Key_i, std::string Symbol, std::string Value,
//                                  contnav_srvs::TABLE_CONTORLLER &tableContorllerService) {
//
//        //1. where 条件
//        contnav_msgs::Dictionaries dictionaries;
//        dictionaries.Condition = Condition;
//        dictionaries.Key_i = Key_i;
//        dictionaries.Symbol = Symbol;
//        dictionaries.Value = Value;
//        tableContorllerService.request.request_where_dictionariesVector.DictionariesVectors.push_back(dictionaries);
//
//    }
//
//
////sql封装条件数据
//    void global_planner::mapTOcontnav_srvs(map <string, string> &dictionarieMap_request_value,
//                                           contnav_srvs::TABLE_CONTORLLER &tableContorllerService) {
//
//        map<string, string>::iterator iter_value;
//        for (iter_value = dictionarieMap_request_value.begin();
//             iter_value != dictionarieMap_request_value.end(); iter_value++) {
//            contnav_msgs::Dictionaries dictionaries;
//            dictionaries.Key_i = iter_value->first;
//            dictionaries.Value = iter_value->second;
//            //  cout << " =========type==\"value\" || type==\"all\"======="  << endl;
//
//            cout << iter_value->first << " : " << iter_value->second << endl;
//            tableContorllerService.request.request_value_dictionariesVector.DictionariesVectors.push_back(dictionaries);
//        }
//
//    }

//多线程 处理状态机
    void global_planner::thread_msg() {
        //ros::Duration(1.0).sleep();
        ros::Rate r(10);
        while (ros::ok()) {
            state_machine_.execute();
            ros::spinOnce();
            // ros::spinOnce();
            //std::this_thread::sleep_for(std::chrono::milliseconds(100));
            r.sleep();

         //   std::cout << " 1.thread_msg（） 通知数据已准备完毕  current_state:"<<state_machine_.get_current_state() << std::endl;
            if(state_machine_.get_current_state()=="monitor_progress"){
                // 通知数据已准备完毕
                state_machine_.notifyDataReady();
                std::cout << " 2.thread_msg（） 通知数据已准备完毕  current_state:"<<state_machine_.get_current_state() << std::endl;

            }

          //  std::cout << " 3.thread_msg（） 通知数据已准备完毕  current_state:"<<state_machine_.get_current_state() << std::endl;


        }
    }




//刷新数据
    void global_planner::callBack_refreshdb(std_msgs::String data) {
        try {
            std::cout << "====刷新数据===global_planner====callBack_refreshdb===data.data====" << std::endl;
           // queryAllFromTABLE_DICTIONARES();
        } catch (std::exception e) {
            e.what();
        }
    }

//定时执行
    void global_planner::updatefun(const ros::TimerEvent &time_event) {
        try {

            timer2_.stop();
            //查询字典数据
          //  queryAllFromTABLE_DICTIONARES();


            std::cout << "============1=====定时执行=global_planner::updatefun===================" << std::endl;
        } catch (exception e) {
            cout << e.what() << endl;
        }
    }


    //service
    bool global_planner::callback_global_plannerFun(contnav_srvs::global_planner::Request &request,
                                                    contnav_srvs::global_planner::Response &response) {


        std::cout << "1.路径规划 ========================callback_global_plannerFun========================== " <<request.request_type<< std::endl;

        //互锁
       // m_.lock();
        //使用： 自动解锁
        AutoLock lock(&m_mutex);
        bool fig = true;
        std::cout << "2.路径规划 callback_global_plannerFun " <<request.request_type<<" : " <<request.makePlan<<std::endl;
        string callback_global_planner_content= "获取路径规划 callback_global_plannerFun() request_type : "+request.request_type+" ，request.request_plan_type： "+request.request_plan_type
                                                                                                                                   +" ，request.request_BezierNum： "+request.request_BezierNum
                                                                                                                                   +" ,request_isPlanArc: "+(request.request_isPlanArc?"true" : "false")
                                                                                                                                   +" ,isSendGoalPlanOverturn: "+(request.isSendGoalPlanOverturn?"true" : "false")
                                                                                                                                   +" ,isOneWay: "+(request.isOneWay?"true" : "false")
                                                                                                                                   +" ,isthresholdClosestPoint: "+(request.isthresholdClosestPoint?"true" : "false")
                                                                                                                                   +" ,request_algorithm: "+request.request_algorithm
                                                                                                                                   +" ,waypointAction: "+to_string(request.waypointAction)
                                                                                                                                   +" ,waypointAction: "+request.request_algorithm
                                                                                                                                 ;

        std::cout << "callback_global_plannerFun: callback_global_planner_content " <<callback_global_planner_content<<std::endl;
        response.message="start 路径规划 callback_global_plannerFun: " +request.request_type;
        //日志
        contnav_debug(__FILE__,__LINE__,const_cast<char*>(response.message.c_str()));
        //todo 初始化全局规划器
        initialize();//初始化

        if(!initialized_||!grid_ready_||!grid_updated_ready_){
            ROS_ERROR("3.路径规划 未初始化 The planner has not been initialized, please call initialize() to use the planner");
            //日志
            //  contnav_error(__FILE__,__LINE__,const_cast<char*>(response.message.c_str()));
            contnav_error(__FILE__,__LINE__,"路径规划 未初始化 The planner has not been initialized, please call initialize() to use the planner");
            return false;
        }
        std::cout << "2.路径规划 ========================callback_global_plannerFun========================== " <<request.request_type<< std::endl;

        try {
         //todo 1.start-----------------------makePlan 路径规划 ----a* d* 全局路径规划-----------------------------------
        if(request.request_type==request.makePlan){//路径规划


            std::cout << "3.路径规划 ================================================== " <<request.request_algorithm<<" : " <<request.AStar<< std::endl;

            unsigned int cx = origin_costmap_->getSizeInCellsX(), cy = origin_costmap_->getSizeInCellsY();
            std::cout << "4.路径规划 ================================================== " << std::endl;
            //todo 2.选择规划算法
            if(request.request_algorithm==request.Dijkstra){//Dijkstra AStar
                std::cout << "路径规划 makePlan Dijkstra 我的全局规划路径 initialize " << std::endl;
                if(planner_!=NULL)
                    delete planner_;
                 DijkstraExpansion* de = new DijkstraExpansion(p_calc_, cx, cy);//true
                de->setPreciseStart(true);
                planner_ = de;
            }else if(request.request_algorithm==request.AStar){//AStar
                std::cout << "路径规划 makePlan AStar 我的全局规划路径 initialize " << std::endl;
                if(planner_!=NULL)
                    delete planner_;
                planner_ = new AStarExpansion(p_calc_, cx, cy);//false  // cx, cy 地图的大小，即像素数。
            }else {
                std::cout << "未选择规划算法 默认 Dijkstra ，路径规划 makePlan Dijkstra 我的全局规划路径 initialize " << std::endl;
                if(planner_!=NULL)
                    delete planner_;
                DijkstraExpansion* de = new DijkstraExpansion(p_calc_, cx, cy);//true
                de->setPreciseStart(true);
                planner_ = de;

                response.message=" 未选择规划算法 默认 Dijkstra！ ";

            }
            std::cout << "6.路径规划 ================================================== " << std::endl;
             //起始点坐标
            if(request.request_start.header.frame_id.empty())
            {
                geometry_msgs::PoseStamped global_pose;
                if(!getRobotPose(global_pose)){
                    response.result = ServiceResultType::error;
                    ROS_ERROR("move_base cannot make a plan for you because it could not get the start pose of the robot");
                    response.message=" error 路径规划 makePlan getRobotPose 未获取到机器人当前位置！ ";
                    response.result = ServiceResultType::error;

                    //日志
                    //  contnav_error(__FILE__,__LINE__,const_cast<char*>(response.message.c_str()));
                    //  contnav_error(__FILE__,__LINE__,"text");
                    return true;
                }
                request.request_start = global_pose;
            }
            std::cout << "7.路径规划 ================================================== " << std::endl;

             planner_->setHasUnknown(allow_unknown_);
             std::vector<geometry_msgs::PoseStamped> plan;
            //目标坐标
            request.request_goal.header.stamp = ros::Time::now();
            request.request_goal.header.frame_id = frame_id_;
            // todo 测试数据
            request.request_goal.pose.position.x = 0.814;
            request.request_goal.pose.position.y = 3.66;
            request.request_goal.pose.position.z = 0.0;
            request.request_goal.pose.orientation.x = 0.0;
            request.request_goal.pose.orientation.y = 0.0;
            request.request_goal.pose.orientation.z = 0.0;
            request.request_goal.pose.orientation.w = 1.0;

           if(makePlan(request.request_start,  request.request_goal,plan)){
               std::cout << "sussed 路径规划 makePlan my_global_planner_plugin 我的全局规划路径  " << std::endl;
               response.message=" 路径规划 makePlan 已经规划出路径来！ ";
               response.result = ServiceResultType::ResultTypeOK;
               response.result_plan=plan;//todo 返回路径

               //日志
                 contnav_info(__FILE__,__LINE__,const_cast<char*>(response.message.c_str()));
               //  contnav_info(__FILE__,__LINE__,"text");
           }else{
               std::cout << "error 路径规划 makePlan my_global_planner_plugin 我的全局规划路径  " << std::endl;
               response.message="error 路径规划 makePlan 未规划出路径来！ ";
               response.result = ServiceResultType::error;
               //日志
                 contnav_error(__FILE__,__LINE__,const_cast<char*>(response.message.c_str()));
               //  contnav_error(__FILE__,__LINE__,"text");
               fig=false;
           }
        }
        //end-------a* d* 全局路径规划---------------------------------------------------------------------------------
        //todo 2.start-----------------------Bezier 贝塞尔------------------------------------------------------------
        else if(request.request_type==request.Bezier){//贝塞尔曲线

            vector<double> ps;   //起始点坐标
            vector<double> pc1;   //控制点1坐标
            vector<double> pc2;//控制点2坐标
            vector<double> pc3;//控制点2坐标
            vector<double> pe;   //终点坐标
            std::vector<geometry_msgs::PoseStamped> plan;//生成的路径

            //todo 测试数据 =========================测试数据==============================

            //目标坐标
            request.request_goal.header.stamp = ros::Time::now();
            request.request_goal.header.frame_id = frame_id_;
            // todo 测试数据
            request.request_goal.pose.position.x = -0.876;
            request.request_goal.pose.position.y = 1.0;
            request.request_goal.pose.position.z = 0.0;
            request.request_goal.pose.orientation.x = 0.0;
            request.request_goal.pose.orientation.y = 0.0;
            request.request_goal.pose.orientation.z = 0.0;
            request.request_goal.pose.orientation.w = 1.0;
            //-----
            //控制点1坐标
            request.request_pc1.header.stamp = ros::Time::now();
            request.request_pc1.header.frame_id = frame_id_;
            request.request_pc1.pose.position.x = -2.2;
            request.request_pc1.pose.position.y = 0.3;
            request.request_pc1.pose.position.z = 0.0;
            request.request_pc1.pose.orientation.x = 0.0;
            request.request_pc1.pose.orientation.y = 0.0;
            request.request_pc1.pose.orientation.z = 0.0;
            request.request_pc1.pose.orientation.w = 1.0;
            //-----
            //控制点2坐标
            request.request_pc2.header.stamp = ros::Time::now();
            request.request_pc2.header.frame_id = frame_id_;
            request.request_pc2.pose.position.x = -1.8;
            request.request_pc2.pose.position.y = 0.4;
            request.request_pc2.pose.position.z = 0.0;
            request.request_pc2.pose.orientation.x = 0.0;
            request.request_pc2.pose.orientation.y = 0.0;
            request.request_pc2.pose.orientation.z = 0.0;
            request.request_pc2.pose.orientation.w = 1.0;
            //控制点3坐标
            request.request_pc3.header.stamp = ros::Time::now();
            request.request_pc3.header.frame_id = frame_id_;
            request.request_pc3.pose.position.x = -1.67;
            request.request_pc3.pose.position.y = 0.2;
            request.request_pc3.pose.position.z = 0.0;
            request.request_pc3.pose.orientation.x = 0.0;
            request.request_pc3.pose.orientation.y = 0.0;
            request.request_pc3.pose.orientation.z = 0.0;
            request.request_pc3.pose.orientation.w = 1.0;



            //todo=======================================================


            //起始点坐标
            if(request.request_start.header.frame_id.empty())
            {
                geometry_msgs::PoseStamped global_pose;
                if(!getRobotPose(global_pose)){
                    response.result = ServiceResultType::error;
                    ROS_ERROR("move_base cannot make a plan for you because it could not get the start pose of the robot");
                    response.message=" error 路径规划 makePlan getRobotPose 未获取到机器人当前位置！ ";
                    //m_.unlock();
                    //日志
                    contnav_error(__FILE__,__LINE__,const_cast<char*>(response.message.c_str()));
                    return true;
                }
                request.request_start = global_pose;
            }

            //把开始坐标加入到队列中
            geometry_msgs::PoseStamped start_pose;
            start_pose.pose.orientation.x = 0.0;
            start_pose.pose.orientation.y = 0.0;
            start_pose.pose.orientation.z = 0.0;
            start_pose.pose.orientation.w = 1.0;
            //设置 geometry_msgs::PoseStamped数据保存到plan路径中
            setPoseStampedPoseToPlan(start_pose,request.request_start.pose.position.x,request.request_start.pose.position.y, plan);

            //todo 1.起始点坐标世界坐标转像素坐标 [需要计算点数及t值]
            unsigned int world_i_start_x, world_i_start_y;//起始点坐标（像素）

            //世界坐标转成像素坐标
            if (!origin_costmap_->worldToMap(request.request_start.pose.position.x, request.request_start.pose.position.y, world_i_start_x, world_i_start_y)) {
                ROS_ERROR("世界坐标转成像素坐标");
                response.message = "error 世界坐标转成像素坐标";
                response.result = ServiceResultType::error;
                //日志
                contnav_error(__FILE__,__LINE__,const_cast<char*>(response.message.c_str()));
                fig = false;
                return true;
            }

            ps.push_back((double)world_i_start_x);
            ps.push_back((double)world_i_start_y);


            //todo 2.控制点1坐标
            geometry_msgs::PoseStamped  pc1_poseStamped = request.request_pc1;
            //控制点1坐标坐标世界坐标转像素坐标 [需要计算点数及t值]
            unsigned int world_i_pc1_x, world_i_pc1_y;
            if (!origin_costmap_->worldToMap(request.request_pc1.pose.position.x, request.request_pc1.pose.position.y, world_i_pc1_x, world_i_pc1_y)) {
                ROS_ERROR("世界坐标转成像素坐标");
                response.message = "error 世界坐标转成像素坐标";
                response.result = ServiceResultType::error;
                //日志
                contnav_error(__FILE__,__LINE__,const_cast<char*>(response.message.c_str()));
                fig = false;
                return true;
            }
            pc1.push_back((double)world_i_pc1_x);
            pc1.push_back((double)world_i_pc1_y);

            //todo 3.目标坐标 （像素）
            unsigned int world_i_goal_x, world_i_goal_y;
            //世界坐标转像素坐标
            if (!origin_costmap_->worldToMap(request.request_goal.pose.position.x, request.request_goal.pose.position.y, world_i_goal_x, world_i_goal_y)){
                ROS_ERROR("世界坐标转成像素坐标");
                response.message = "error 世界坐标转成像素坐标";
                response.result = ServiceResultType::error;
                //日志
                contnav_error(__FILE__,__LINE__,const_cast<char*>(response.message.c_str()));
                fig = false;
                return true;
            }
            pe.push_back((double)world_i_goal_x);
            pe.push_back((double)world_i_goal_y);


            //todo 4.计算贝塞尔曲线的点数及t值，先计算两点之间的距离，然后根据距离计算点数，然后计算t值
            int distance;
            std::cout<<"0.贝塞尔 distance: "<<distance<<std::endl;
            if(request.request_BezierNum==request.FirstBezier){//一阶贝塞尔
                // 通过开始点与控制点计算两点直接的像素直合宿据 Calculate the distance between two points using the Pythagorean theorem
                 distance =sqrt(pow((int)world_i_goal_x - (int)world_i_start_x, 2) + pow((int)world_i_goal_y - (int)world_i_start_y, 2));
                std::cout<<"00.贝塞尔 distance: "<<distance<<std::endl;
            }else{
                // 通过开始点与控制点计算两点直接的像素直合宿据 Calculate the distance between two points using the Pythagorean theorem
                 distance =sqrt(pow((int)world_i_pc1_x - (int)world_i_start_x, 2) + pow((int)world_i_pc1_y - (int)world_i_start_y, 2));
                std::cout<<"000.贝塞尔 distance: "<<distance<<std::endl;

            }



            std::cout<<"1.贝塞尔 world_i_start_x: "<<world_i_start_x<<std::endl;
            std::cout<<"1.贝塞尔 world_i_start_y: "<<world_i_start_y<<std::endl;

            std::cout<<"1.贝塞尔 world_i_pc1_x: "<<world_i_pc1_x<<std::endl;
            std::cout<<"1.贝塞尔 world_i_pc1_y: "<<world_i_pc1_y<<std::endl;

            std::cout<<"1.贝塞尔 world_i_goal_x: "<<world_i_goal_x<<std::endl;
            std::cout<<"1.贝塞尔 world_i_goal_y: "<<world_i_goal_y<<std::endl;

          //  std::cout<<"1.贝塞尔 pow(world_i_goal_x - world_i_start_x, 2): "<<pow(world_i_goal_x - world_i_start_x, 2)<<std::endl;
            std::cout<<"1.贝塞尔 pow(world_i_goal_y: "<<world_i_goal_y<<" - world_i_start_y:"<<world_i_start_y<<", 2): "<<pow((int)world_i_goal_y - (int)world_i_start_y, 2)<<std::endl;


           std::cout<<"1.贝塞尔 distance: "<<distance<<std::endl;
            if(request.request_BezierNum==request.FirstBezier){//一阶贝塞尔
                std::cout<<"2.一阶贝塞尔"<<std::endl;
                // distance 个点
                for (int i = 1; i < distance; i++)
                {
                    double t =(double)  i / (double) distance;
                    vector<double> result = FirstBezier(ps, pe,  t);
                   // vector<int> result = FirstBezier(ps, pe, t);
                    // geometry_msgs::PoseStamped pose;
                    double world_x, world_y;
                    //像素坐标转世界坐标
                    mapToWorld(result[0], result[1], world_x, world_y);
                    geometry_msgs::PoseStamped pose;
                    pose.pose.orientation.x = 0.0;
                    pose.pose.orientation.y = 0.0;
                    pose.pose.orientation.z = 0.0;
                    pose.pose.orientation.w = 1.0;
                    //设置 geometry_msgs::PoseStamped数据保存到plan路径中
                    setPoseStampedPoseToPlan(pose,world_x,world_y, plan);

                }
            }
            if(request.request_BezierNum==request.QuadraticBezier){//二阶贝塞尔

                std::cout<<"3.二阶贝塞尔"<<std::endl;

                // distance 个点
                for (int i = 1; i < distance; i++)
                {
                    double t =(double)  i / (double) distance;
                    vector<double> result = QuadraticBezier(ps, pc1, pe, t);
                    // geometry_msgs::PoseStamped pose;
                    double world_x, world_y;
                    //像素坐标转世界坐标
                    mapToWorld(result[0], result[1], world_x, world_y);
                    geometry_msgs::PoseStamped pose;
                    pose.pose.orientation.x = 0.0;
                    pose.pose.orientation.y = 0.0;
                    pose.pose.orientation.z = 0.0;
                    pose.pose.orientation.w = 1.0;
                    //设置 geometry_msgs::PoseStamped数据保存到plan路径中
                    setPoseStampedPoseToPlan(pose,world_x,world_y, plan);
                }

            }
            if(request.request_BezierNum==request.CubicBezier){//三阶贝塞尔
                std::cout<<"4.三阶贝塞尔"<<std::endl;
                //控制点2坐标
               // geometry_msgs::PoseStamped  pc2_poseStamped = request.request_pc2;
                //控制点2坐标坐标世界坐标转像素坐标
                unsigned int world_i_pc2_x, world_i_pc2_y;//控制点2坐标（像素）
                if (!origin_costmap_->worldToMap(request.request_pc2.pose.position.x, request.request_pc2.pose.position.y, world_i_pc2_x, world_i_pc2_y)){
                    ROS_ERROR("世界坐标转成像素坐标");
                    response.message = "error 世界坐标转成像素坐标";
                    response.result = ServiceResultType::error;
                    fig = false;
                    //日志
                    contnav_error(__FILE__,__LINE__,const_cast<char*>(response.message.c_str()));
                    return true;
                }
                pc2.push_back((int)world_i_pc2_x);
                pc2.push_back((int)world_i_pc2_y);
                // distance 个点
                for (int i = 1; i < distance; i++)
                {
                    double t =(double)  i / (double) distance;
                    vector<double> result = CubicBezier(ps, pc1,pc2, pe, t);
                    // geometry_msgs::PoseStamped pose;
                    double world_x, world_y;
                    //像素坐标转世界坐标
                    mapToWorld(result[0], result[1], world_x, world_y);
                    geometry_msgs::PoseStamped pose;
                    pose.pose.orientation.x = 0.0;
                    pose.pose.orientation.y = 0.0;
                    pose.pose.orientation.z = 0.0;
                    pose.pose.orientation.w = 1.0;
                    //设置 geometry_msgs::PoseStamped数据保存到plan路径中
                    setPoseStampedPoseToPlan(pose,world_x,world_y, plan);
                }

            }

            if(request.request_BezierNum==request.EquationBezier){//四阶贝塞尔
                std::cout<<"5.四阶贝塞尔"<<std::endl;

                //控制点2坐标
               // geometry_msgs::PoseStamped  pc2_poseStamped = request.request_pc2;
                //控制点2坐标坐标世界坐标转像素坐标
                unsigned int world_i_pc2_x, world_i_pc2_y;//控制点2坐标（像素）
                if (!origin_costmap_->worldToMap(request.request_pc2.pose.position.x, request.request_pc2.pose.position.y, world_i_pc2_x, world_i_pc2_y)){
                    ROS_ERROR("世界坐标转成像素坐标");
                    response.message = "error 世界坐标转成像素坐标";
                    response.result = ServiceResultType::error;
                    fig = false;
                    //日志
                    contnav_error(__FILE__,__LINE__,const_cast<char*>(response.message.c_str()));
                    return true;
                }
                pc2.push_back((int)world_i_pc2_x);
                pc2.push_back((int)world_i_pc2_y);
                //控制点3坐标
                //geometry_msgs::PoseStamped  pc3_poseStamped = request.request_pc3;
                //控制点3坐标坐标世界坐标转像素坐标
                unsigned int world_i_pc3_x, world_i_pc3_y;//控制点3坐标（像素）
                if (!origin_costmap_->worldToMap(request.request_pc3.pose.position.x, request.request_pc3.pose.position.y, world_i_pc3_x, world_i_pc3_y)){
                    ROS_ERROR("世界坐标转成像素坐标");
                    response.message = "error 世界坐标转成像素坐标";
                    response.result = ServiceResultType::error;
                    fig = false;
                    //日志
                     contnav_error(__FILE__,__LINE__,const_cast<char*>(response.message.c_str()));
                    return true;
                }
                pc3.push_back((int)world_i_pc3_x);
                pc3.push_back((int)world_i_pc3_y);
                // distance 个点
                for (int i = 1; i < distance; i++)
                {
                     double t =(double)  i / (double) distance;
                    vector<double> result = EquationBezier(ps, pc1,pc2, pc3,  pe, t);
                    // geometry_msgs::PoseStamped pose;
                    double world_x, world_y;
                    //像素坐标转世界坐标
                    mapToWorld(result[0], result[1], world_x, world_y);
                    geometry_msgs::PoseStamped pose;
                    pose.pose.orientation.x = 0.0;
                    pose.pose.orientation.y = 0.0;
                    pose.pose.orientation.z = 0.0;
                    pose.pose.orientation.w = 1.0;
                    //设置 geometry_msgs::PoseStamped数据保存到plan路径中
                    setPoseStampedPoseToPlan(pose,world_x,world_y, plan);
                }


            }

            std::cout<<"6 plan.size() "<<plan.size()<<std::endl;

            //把终点坐标加入到队列中
            geometry_msgs::PoseStamped goal_pose;
            goal_pose.pose.orientation.x = 0.0;
            goal_pose.pose.orientation.y = 0.0;
            goal_pose.pose.orientation.z = 0.0;
            goal_pose.pose.orientation.w = 1.0;
            //设置 geometry_msgs::PoseStamped数据保存到plan路径中
            setPoseStampedPoseToPlan(goal_pose,request.request_goal.pose.position.x,request.request_goal.pose.position.y, plan);

            if(plan.size()>2){
                // add orientations if needed 如果需要，添加方向
                orientation_filter_->processPath(request.request_start, plan);

                //publish the plan for visualization purposes 出于可视化目的发布计划
                //std::vector<geometry_msgs::PoseStamped> plan;
                publishPlan(plan,response.result_Path);
                ROS_INFO("贝塞尔 路径规划成功");
                response.message="贝塞尔 路径规划成功 ";
                response.result_plan=plan;//tood 返回路径
                response.result = ServiceResultType::ResultTypeOK;
                //日志
                contnav_info(__FILE__,__LINE__,const_cast<char*>(response.message.c_str()));
            }else{
                ROS_ERROR("贝塞尔 路径规划失败");
                std::cout << "贝塞尔 路径规划失败  " << std::endl;
                response.message="贝塞尔 路径规划失败 ";
                response.result = ServiceResultType::error;
                //日志
                 contnav_error(__FILE__,__LINE__,const_cast<char*>(response.message.c_str()));
                fig=false;
            }


            //----
        }
        //end------------------------贝塞尔 路径规划--------------------------------------------------------------------
        //todo 3.start --------------------获取rviz登界面上取的点，或者从数据库取点 规划路径----------------------------------
        else if(request.request_type==request.waypoint) {

            if(request.waypointAction==request.CALCULATEPATH){//计算一条路径
                response.message="waypoint 计算一条路径 ";
                std::vector<geometry_msgs::PoseStamped> plan;//生成的路径
                nav_msgs::Path gui_path;//生成的路径
                nav_msgs::Path gui_arc_path;//生成的路径,带圆弧
                if (waypoints_.size() < 2) {
                    waypoints_.clear();//清除数据

                    ROS_ERROR("waypoints_.size()<2");
                    response.message = "error waypoints_.size()<2 获取rviz登界面上取的点，或者从数据库取点 规划路径";
                    response.result = ServiceResultType::error;
                    //日志
                    contnav_error(__FILE__,__LINE__,const_cast<char*>(response.message.c_str()));
                    fig = false;
                    return true;
                }

                //计算两条线段交点处的圆弧路径使用
                if(onePlanToMaps_vector_.size()>0) //getline函数中计算onePlanToMaps_vector_
                    onePlanToMaps_vector_.clear();

                ROS_ERROR("waypoints_.size()  %i",waypoints_.size());
                geometry_msgs::PoseStamped start=waypoints_[0];
                 //todo 显示路径箭头
                visualization_msgs::Marker markerARROW;
                markerARROW.header.frame_id = frame_id_;
                markerARROW.header.stamp = ros::Time::now();
                markerARROW.ns = "publishMarkerPlan";//以后改成当前地图名称 ，命名空间 用当前地图名称+场景num，从数据库读取
                // %Tag(ACTION)%
                markerARROW.action = visualization_msgs::Marker::ADD;
                //计算路径的朝向，计算向量的斜率，向量的朝向 方向
                double angle;
                tf2::Quaternion q=getSlopeOfVector(start.pose.position.x, start.pose.position.y,waypoints_[1].pose.position.x, waypoints_[1].pose.position.y,angle);
                markerARROW.type = visualization_msgs::Marker::ARROW;    //visualization_msgs::Marker::ARROW 0 箭头
                markerARROW.pose.position.x = start.pose.position.x;
                markerARROW.pose.position.y = start.pose.position.y;
                markerARROW.pose.position.z = start.pose.position.z;
                markerARROW.pose.orientation.x =  q.x();
                markerARROW.pose.orientation.y =  q.y();
                markerARROW.pose.orientation.z =  q.z();
                markerARROW.pose.orientation.w =  q.w();
                markerARROW.color.r = 1.0f;
                markerARROW.color.g = 1.0f;
                markerARROW.color.a = 1.0;
                markerARROW.scale.x = 0.2;
                markerARROW.scale.y = 0.2;
                markerARROW.scale.z = 0.2;
                markerARROW.id = markerId++;
                //marker.lifetime = ros::Duration();
                marker_pub_.publish(markerARROW);
                marker_arc_pub_.publish(markerARROW);
                ros::Duration(0.05).sleep(); //todo 需要延时一下才会显示箭头 方向
                for (int i = 1; i < waypoints_.size(); i++) {//遍历所有的点

                    //---
                    geometry_msgs::PoseStamped end = waypoints_[i];

                    ROS_INFO("start.pose.position.x is %f", start.pose.position.x);
                    ROS_INFO("start.pose.position.y is %f", start.pose.position.y);

                    ROS_INFO("end.pose.position.x is %f", end.pose.position.x);
                    ROS_INFO("end.pose.position.y is %f", end.pose.position.y);
                    ROS_ERROR("   i %i",i);
                    //todo 2个点贝赛尔计算一条直线，多个点连成一条长直线，同时 getlinne函数中会把每一条路径的数据加入到 全局变量 onePlanToMaps_vector_ 集合中；
                    //通过起始点和结束点获取一条 贝赛尔直线 ，waypoints_ 取出点为： startPoseStamped:起始点 endPoseStamped:结束点， 规划得到的 plan:路径，需要显示的gui_path:路径
                   bool getlinefig = getline(start,end,plan,gui_path);//这个函数中 会执行 onePlanToMaps_vector_.push_back(onePlanToMap);//todo [计算两条线段交点处的圆弧路径使用] onePlanToMaps_vector_.push_back(onePlanToMap)
                   if(!getlinefig){
                          ROS_ERROR("获取直线 路径失败");
                          response.message = "error 获取直线 路径失败";
                          response.result = ServiceResultType::error;
                              //日志
                            contnav_error(__FILE__,__LINE__,const_cast<char*>(response.message.c_str()));

                          fig = false;
                       if(waypoints_.size()>0)
                           waypoints_.clear();//清除数据
                          return true;
                   }

                    start=end;

                }
                   response.result_waypointSize=waypoints_.size();

                //----------------------------------------------------------------------
                if(waypoints_.size()>0)
                    waypoints_.clear();//清除数据
                //---------------------------------------------------------------------

                vector<Point>  distance_plan_all_vector;//todo 【保存多个点位生成的一条路径的所有路径点 包含圆弧的路径点】
                vector<Point>  distance_plan_noArc_vector;//todo【保存多个点位生成的一条路径的所有路径点 包含转角的路径点】
                //计算两条线段交点处的圆弧路径使用
                if(onePlanToMaps_vector_.size()>0){//todo 进入这个if(onePlanToMaps_vector_.size()>0) ，global_planner_path_with_rounded_corners 中会计算 ，才有 这两个 distance_plan_all_vector，distance_plan_noArc_vector数据
                    std::vector<geometry_msgs::PoseStamped> planArc;
                    //todo 计算圆弧路径  函数中 调用全局容器 onePlanToMaps_vector_ 获取路径数据；【onePlanToMaps_vector_数据在 函数 getline中进行封装】
                    bool planArcfig =global_planner_path_with_rounded_corners(start,planArc,distance_plan_all_vector,distance_plan_noArc_vector);
                    if(!planArcfig){
                        ROS_ERROR("计算圆弧路径失败");
                        response.message = "error 计算圆弧路径失败";
                        response.result = ServiceResultType::error;
                        //日志
                        contnav_error(__FILE__,__LINE__,const_cast<char*>(response.message.c_str()));
                        fig = false;
                        return true;
                    }


                    if(request.request_isPlanArc){//true:是圆弧路径

                        std::vector<int> plan_indexs;//当前路径在地图中的索引
                       // std::cout<<"start============================request_isPlanArc================================================"<< std::endl;
                        for (int i = 0; i < distance_plan_noArc_vector.size(); ++i) { //计算路径的索引
                            Point point = distance_plan_noArc_vector[i];
                            int  index = toIndex((int)point.x, (int)point.y,nx_,ny_);//将周围点的坐标转化为数组下标。 toIndex(int x, int y,int nx ,int ny)
                            plan_indexs.push_back(index);

//                            std::cout<<"nx_+++++++++++++++++++++++++++++++++++++++++++++++  point.x     :"<<std::to_string(point.x)<< std::endl;
//                            std::cout<<"ny_+++++++++++++++++++++++++++++++++++++++++++++++  point.y     :"<<std::to_string(point.y)<< std::endl;
//
//                            std::cout<<"nx_+++++++++++++++++++++++++++++++++++++++++++++++  nx_     :"<<std::to_string(nx_)<< std::endl;
//                            std::cout<<"ny_+++++++++++++++++++++++++++++++++++++++++++++++  ny_     :"<<std::to_string(ny_)<< std::endl;
//                            std::cout<<"ny_+++++++++++++++++++++++++++++++++++++++++++++++  index     :"<<std::to_string(index)<< std::endl;


                        }
                     //   std::cout<<"end============================================================================"<< std::endl;

                        //todo 需要执行的路径都依次放进容器里
                        string plan_type=request.request_plan_type;;//路径规划的类型
                        int plan_id=request.request_plan_id;//当前路径id，唯一id
                        string plan_name=request.request_plan_name;//当前路径name，随意起名称
                        string ns=request.request_plan_ns;//当前路径 namespace ，用当前地图名称+场景编号作命名空间
                        std::vector<int> target_plan_ids=request.request_target_plan_ids;//当前路径行走结束后，可以到达的路径id
                          // 圆弧路径 distance_plan_all_vector 像素坐标
                        MorPlanToMap onePlanToMap(plan_type,plan_id,plan_name,target_plan_ids,planArc,distance_plan_all_vector,plan_indexs);//多个路径的容器
                       // execute_plan_.push_back(onePlanToMap);//需要执行的多个路径 带圆弧
                        execute_map_plan_[plan_id]=onePlanToMap;//todo 需要执行的多个路径 带圆弧 [通过这里的数据来实现半结构化路径规划]
                        std::cout << "0.001 if(!request.request_isPlanArc){//false:不走圆弧路径  execute_map_plan_ : plan_id: " <<plan_id<<std::endl;

                    }



                   // marker_arc_pub_
                    //plan=planArc;

                    //1.路径可视化 带圆弧
                    visualization_msgs::Marker markerArc; //std::vector<geometry_msgs::PoseStamped> planArc; nav_msgs::Path gui_arc_path;//生成的路径,带圆弧
                    bool publishPlanMarkerFunfig1 =  publishPlanMarkerFun(markerArc,planArc,gui_arc_path);
                    if(publishPlanMarkerFunfig1){



                        //发布可视化
                        plan_arc_pub_.publish(gui_path);//发布路径 带圆弧
                        marker_arc_pub_.publish(markerArc);//发布多路径 带圆弧
                        //文字反了
                        // /**
                        ros::Duration(0.1).sleep(); //todo 需要延时一下才会显示箭头
                        visualization_msgs::Marker markertext;
                        markertext.header.frame_id = frame_id_;///odom  base_link frame_id_
                        markertext.header.stamp = ros::Time::now();
                        markertext.type = visualization_msgs::Marker::TEXT_VIEW_FACING; //visualization_msgs::Marker::CUBE 6 立方体序列
                        markertext.action = visualization_msgs::Marker::ADD;
                        markertext.ns = "publishMarkerPlan";//以后改成当前地图名称 ，命名空间 用当前地图名称+场景num，从数据库读取
                        markertext.id = markerId++;
                        markertext.pose.position.x = markerARROW.pose.position.x ;
                        markertext.pose.position.y = markerARROW.pose.position.y +0.8;
                        markertext.pose.position.z = 0 ;
//                        markertext.pose.position.x =  markerARROW.pose.position.x  ;
//                        markertext.pose.position.y = 0;
//                        markertext.pose.position.z = 0 ;
//                        markertext.pose.orientation.x = 0.4999998;
//                        markertext.pose.orientation.y = -0.4996018;
//                        markertext.pose.orientation.z = 0.4999998;
//                        markertext.pose.orientation.w = 0.5003982;
                        markertext.pose.orientation.w =1.0;
                        markertext.color.r = 0.0f;
                        markertext.color.g = 0.0f;
                        markertext.color.b = 1.0f;
                        markertext.color.a = 1.0;
                        // %Tag(SCALE)%
                        markertext.scale.x = 1.1;
                        markertext.scale.y = 1.1;
                        markertext.scale.z = 1.1;
                       // markertext.text = "带圆弧路径成功";//to_string(markerArc.id);
                        // markerArc.id;
                        // 通过这个visualization_msgs::Marker详细发布visualization_msgs::Marker::TEXT_VIEW_FACING文字显示，发现显示的文字颠倒了，调整哪个参数可以把文字转动180度
                        markertext.text = to_string(request.request_plan_id);//request.request_plan_id   to_string(markerArc.id);
                        marker_arc_pub_.publish(markertext);
                        //*/
                        ROS_ERROR("Marker gui_arc_path.poses.size()i %i",gui_arc_path.poses.size());
                        ROS_ERROR("Marker带圆弧 planArc.size()i %i",planArc.size());
                        //ROS_ERROR("path_with_rounded.size()i %i",path_with_rounded.size());
                        ROS_INFO("-------Marker 获取直线 带圆弧路径成功---------------------------");
                        response.message=" Marker 获取直线 带圆弧路径成功 ";
                        //return true;
                    }else{
                        ROS_ERROR("Marker 获取直线 带圆弧路径失败 ");
                        std::cout << "Marker 获取直线 带圆弧路径失败  " << std::endl;
                        response.message="Marker 获取直线 带圆弧路径失败 ";
                        return false;
                    }
                    ros::Duration(0.05).sleep(); //todo 需要延时一下才会显示箭头
                }
                //-----------

                if(!request.request_isPlanArc){//false:不走圆弧路径

                    std::vector<int> plan_indexs;//当前路径在地图中的索引
                 //   std::cout<<"start============================plan_indexs================================================"<< std::endl;

                    for (int i = 0; i < distance_plan_noArc_vector.size(); ++i) { //计算路径在地图中的索引
                        Point point = distance_plan_noArc_vector[i];
                        int  index = toIndex((int)point.x, (int)point.y,nx_,ny_);//将周围点的坐标转化为数组下标。 toIndex(int x, int y,int nx ,int ny)
                        plan_indexs.push_back(index);
//                        std::cout<<"nx_+++++++++++++++++++++++++++++++++++++++++++++++  point.x     :"<<std::to_string(point.x)<< std::endl;
//                        std::cout<<"ny_+++++++++++++++++++++++++++++++++++++++++++++++  point.y     :"<<std::to_string(point.y)<< std::endl;
//
//                        std::cout<<"nx_+++++++++++++++++++++++++++++++++++++++++++++++  nx_     :"<<std::to_string(nx_)<< std::endl;
//                        std::cout<<"ny_+++++++++++++++++++++++++++++++++++++++++++++++  ny_     :"<<std::to_string(ny_)<< std::endl;
//                        std::cout<<"ny_+++++++++++++++++++++++++++++++++++++++++++++++  index     :"<<std::to_string(index)<< std::endl;

                    }
                 //   std::cout<<"end================================================================================================="<< std::endl;

                    //todo -----------------需要执行的路径都依次放进容器里---------------------------
                    //------------------------------------------------------------
                    string plan_type=request.request_plan_type;;//路径规划的类型
                    int plan_id=request.request_plan_id;//当前路径id，唯一id
                    string plan_name=request.request_plan_name;//当前路径name，随意起名称
                    string ns=request.request_plan_ns;//当前路径 namespace ，用当前地图名称+场景编号作命名空间
                    std::vector<int> target_plan_ids=request.request_target_plan_ids;//当前路径行走结束后，可以到达的路径id
                   //todo 多个路径的容器  带转角路径 distance_plan_noArc_vector 像素坐标 （转角未处理圆弧）  //   std::vector<geometry_msgs::PoseStamped> plan;//生成的路径
                    MorPlanToMap onePlanToMap(plan_type,plan_id,plan_name,target_plan_ids,plan,distance_plan_noArc_vector,plan_indexs);

                    //todo 多个路径的容器
                   // execute_plan_.push_back(onePlanToMap);//需要执行的多个路径
                    execute_map_plan_[plan_id]=onePlanToMap;//需要执行的多个路径，把所有半结构化路径放容器中

                    std::cout << "0.002 if(!request.request_isPlanArc){//false:不走圆弧路径  execute_map_plan_ : plan_id: " <<plan_id<<std::endl;

                }




                   //2.路径可视化 转角不变
                visualization_msgs::Marker markerangle;
                bool publishPlanMarkerFunfig =  publishPlanMarkerFun(markerangle,plan,gui_path);
                if(publishPlanMarkerFunfig){

                    markerangle.color.r = 1.0f;
                    markerangle.color.g = 0.0f;
                    markerangle.color.b = 0.0f;
                    markerangle.color.a = 1.0;
                    // %Tag(SCALE)%
                    markerangle.scale.x = 0.05;
                    markerangle.scale.y = 0.05;
                    markerangle.scale.z = 0.05;


                    plan_pub_.publish(gui_path);//发布路径
                    marker_pub_.publish(markerangle);//发布多路径
                    ROS_ERROR("gui_path.poses.size()i %i",gui_path.poses.size());
                    ROS_ERROR("plan.size()i %i",plan.size());
                    //ROS_ERROR("path_with_rounded.size()i %i",path_with_rounded.size());
                    ROS_INFO("获取直线 路径成功");
                    response.message="获取直线 路径成功 ";
                  //  return true;
                }else{
                    ROS_ERROR("获取直线 路径失败 ");
                    std::cout << "获取直线 路径失败  " << std::endl;
                    response.message="获取直线 路径失败 ";
                    return false;
                }
                 //多个路径的容器
               // response.result_executePlanSize=execute_plan_.size();
                response.result_executePlanSize=execute_map_plan_.size();

                //打印
                map<int, MorPlanToMap>::iterator iter3;
                for(iter3 = execute_map_plan_.begin(); iter3 != execute_map_plan_.end(); iter3++) {
                    int current_plan_id = iter3->first; //todo 当前路径id，唯一id ；每个路径在数据库中都会存储一个唯一的id
                    std::cout << "0.00000 获取rviz登界面上取的点，或者从数据库取点 规划路径-  execute_map_plan_ : plan_id: " <<current_plan_id<<std::endl;
                }

//                if(state_machine_.get_current_state() == "idle"||state_machine_.get_current_state() == "init_global_waypoints"){
//                    //状态机
//                    state_machine_.set_state("init_global_waypoints", {});//1.选择点位设置全局路径 init_global_waypoints
//                }else{
////                       ROS_ERROR("navigate_to_goal 发送导航失败，情等待状态切换为闲置状态 idle 才能继续导航操作。当前状态Invalid state: %s", state_machine_.get_current_state().c_str() );
////                   }

                //------------------------
            }else  if(request.waypointAction==request.LOADINGPATH){//LOADINGPATH 7 //把所有半结构化路径在地图中的索引重新放在一个容器里-
                response.message="把所有半结构化路径在地图中的索引重新放在一个容器里";
                current_path_goTo_target_path_map_plan_.clear();
                current_path_goTo_target_path_map_plan_Overturn_.clear();
                plan_all_indexs_.clear();//当前路径在地图中的索引
                if(request.isthresholdClosestPoint){ // true:选择最近点作为切入和切出点（切入点与切出点相对固定，取决于起点和终点的位置【满足 切入点最小距离阀值，切出点最小距离阀值的范围内选择】）， false：选择组合路径距离最短的点作为切入和切出点（满足  切入点最小距离阀值，切出点最小距离阀值的范围内选择）
                    isthresholdClosestPoint_=true;
                }else{
                    isthresholdClosestPoint_=false;
                }


                //todo (暂时写死)返回默认点位，或是充电点位
                request.request_default_goal.header.stamp = ros::Time::now();
                request.request_default_goal.header.frame_id = frame_id_;
                request.request_default_goal.pose.position.x = -4.4;
                request.request_default_goal.pose.position.y = 1.11;
                request.request_default_goal.pose.position.z = 0.0;
                request.request_default_goal.pose.orientation.x = 0.0;
                request.request_default_goal.pose.orientation.y = 0.0;
                request.request_default_goal.pose.orientation.z = 0.0;
                request.request_default_goal.pose.orientation.w = 1.0;

                if(request.request_default_goal.header.frame_id==""){
                    string meg="request.request_default_goal.header.frame_id==map";
                    ROS_INFO("默认点位，或是充电点位 必须给值 request.request_default_goal  frame_id_ %s ",frame_id_.c_str());
                    response.message="errror 默认点位，或是充电点位 必须给值 request.request_default_goal frame_id_: %s "+frame_id_;
                    //日志
                    contnav_error(__FILE__,__LINE__,const_cast<char*>(response.message.c_str()));
                    return true;
                }




                //-----------------------------//把所有半结构化路径在地图中的索引重新放在一个容器里-------------------------------------------------------------------
                map<int, MorPlanToMap>::iterator iter;
                for(iter = execute_map_plan_.begin(); iter != execute_map_plan_.end(); iter++) {
                    // cout << iter->first << " : " << iter->second << endl;
                    cout <<"1 callback_global_plannerFun if(request.waypointAction==request.LOADINGPATH) key 当前路径的 index : "<< iter->first  << endl;
                    int current_plan_id=iter->first; //当前路径id，唯一id ；每个路径在数据库中都会存储一个唯一的id

                    MorPlanToMap current_morPlanToMap=  iter->second;//当前的路径
                    std::vector<int> plan_indexs = current_morPlanToMap.plan_indexs;//当前路径在地图中每个点的索引

                    //todo 1. 把所有路径在地图中的索引重新放在一个容器里-
                    for (int i = 0; i < plan_indexs.size(); ++i) {
                        plan_all_indexs_.push_back(plan_indexs[i]);//当前路径在地图中的索引
                    }

                    cout<<"2.****把所有半结构化路径在地图中的索引重新放在一个容器里akePlanWithSemiStructuredPlan.size(): "<<endl;

                    //---
                    //todo 2. 找出所有路径的终点，与其它路径的起点（终点与起点是提前配置好的可行走路径），通过A*规划一条形成路径
                    if(current_morPlanToMap.target_plan_ids.size()==0){
                        ROS_ERROR("当前路径没有目标可行走的路径");
                        response.message="当前路径没有目标可行走的路径 target_plan_ids.size(): "+
                                         to_string(current_morPlanToMap.target_plan_ids.size());
                        //日志
                        contnav_error(__FILE__,__LINE__,const_cast<char*>(response.message.c_str()));
                        return true;
                    }
                    std::vector<int> target_plan_ids = current_morPlanToMap.target_plan_ids;//当前路径行走结束后，可以到达的路径id
                    //取出路径的最后一个点，作为起始点，像素直
                    Point morPlanToMap_startpoint = current_morPlanToMap.distance_plan_all_vector[current_morPlanToMap.distance_plan_all_vector.size()-1];//vector<Point>  distance_plan_all_vector;//todo 保存路径 所有点的像素坐标
                    cout<<"3.****把所有半结构化路径在地图中的索引重新放在一个容器里akePlanWithSemiStructuredPlan.size(): "<<endl;

                    //todo 储存点到点规划的路径 [路径首尾相连的路径]
                    //储存点到点规划的路径，记录key：终点路径的路径索引id 对应的 规划的路径封装的数据结构
                    map<int, CurrentPathGoToTargetPath> target_map_plan;
                    for (int i = 0; i < target_plan_ids.size(); ++i) {// i 是对应路径的路径索引 index；每个路径在数据库中都会存储一个唯一的id

                        //需要判断map中的key是否存在
                        //查找键值为k的元素，判断map中是否存在键值为k的元素
                        auto posKey=execute_map_plan_.find(target_plan_ids[i]);
                        if(posKey==execute_map_plan_.end()){
                            cout<<"需要判断 execute_map_plan_ 中的key是否存在 ；target_plan_ids[i] "<<target_plan_ids[i]<<endl;
                            continue;
                        }
                        //------------------------------------------
                        cout<<"4.****把所有半结构化路径在地图中的索引重新放在一个容器里akePlanWithSemiStructuredPlan.size():"<<endl;

                        MorPlanToMap target_morPlanToMap = execute_map_plan_[target_plan_ids[i]];//当前点可以到达的路径的起点
                        int target_plan_id=target_plan_ids[i]; // i 是对应路径的路径索引 index；每个路径在数据库中都会存储一个唯一的id
                        //当前点可以到达的路径的起点
                        if (target_morPlanToMap.distance_plan_all_vector.size() <= 0) {
                            ROS_ERROR("这条路径的终点没有，可以到达的路径； callback_global_plannerFun if(request.waypointAction==request.LOADINGPATH) ");
                            continue;
                        }

                        //取出路径的第一个点，作为目标点像素直
                        Point target_goal_point = target_morPlanToMap.distance_plan_all_vector[0];//vector<Point>  distance_plan_all_vector;//todo 保存路径 所有点的像素坐标
                        //一条路径 坐标
                        std::vector<geometry_msgs::PoseStamped> makePlanWithSemiStructuredPlan;//todo 保存规划的路径
                        //一条路径 像素
                        std::vector<Point> plan_points;//todo 保存路径 所有点的像素坐标
                        //============================================================================================
                        cout<<"5.****把所有半结构化路径在地图中的索引重新放在一个容器里akePlanWithSemiStructuredPlan.size(): "<<endl;

                        //===========================================================================================
                        //todo 1.使用a*规划，或 使用d*规划；2.一阶贝塞尔 //request.Bezier ,request.makePlan
                        if(request.waypointActionPlanType==request.makePlan){  //路径规划:"makePlan" , 贝赛尔曲线规划:"Bezier" ;当参数waypointAction =CALCULATEPATH=0 计算一条路径，可选择路径规划类型 :makePlan=makePlan 路径规划（取规划器 string request_algorithm 的参数作为当前规划器 ，默认a*），Bezier=Bezier 贝赛尔曲线规划（调用1阶贝赛尔作为直线插值）

                            //start-----------makePlan-------plan-----------
                            string algorithmjubu="AStar";//Dijkstra AStar
                            if(request.request_algorithm==request.Dijkstra)//Dijkstra AStar
                            {
                                algorithmjubu="Dijkstra";
                            }

                            cout<<"6.****把所有半结构化路径在地图中的索引重新放在一个容器里akePlanWithSemiStructuredPlan.size(): "<<endl;

                            //todo morPlanToMap_startpoint ，target_goal_point这两个点用a* 规划一条路径 【每条路径的终点到每条路径的起点】
                            //todo 2.规划路径，半结构路径规划中的 makePlan函数   tolerance =default_tolerance_ //    1451    private_nh2.param("default_tolerance", default_tolerance_, 0.0);//#默认0.0，目标容差
                            bool makePlanWithSemiStructuredPathFig = makePlanWithSemiStructuredPath(morPlanToMap_startpoint,
                                                                                                    target_goal_point,
                                                                                                    algorithmjubu,
                                                                                                    default_tolerance_,
                                                                                                    makePlanWithSemiStructuredPlan);// Dijkstra 算法 ; AStar 算法


                            cout<<"1.****把所有半结构化路径在地图中的索引重新放在一个容器里akePlanWithSemiStructuredPlan.size(): "<<makePlanWithSemiStructuredPlan.size()<<endl;
                            if (!makePlanWithSemiStructuredPathFig)
                                continue;

                            cout<<"2.****把所有半结构化路径在地图中的索引重新放在一个容器里akePlanWithSemiStructuredPlan.size(): "<<makePlanWithSemiStructuredPlan.size()<<endl;


                            // std::vector<Point> plan_points;//todo 保存路径 所有点的像素坐标
                            for (int j = 0; j < makePlanWithSemiStructuredPlan.size(); ++j) {//把坐标转成像素坐标
                                cout<<"8.****把所有半结构化路径在地图中的索引重新放在一个容器里akePlanWithSemiStructuredPlan.size(): "<<endl;

                                double  wx = makePlanWithSemiStructuredPlan[j].pose.position.x;
                                double  wy = makePlanWithSemiStructuredPlan[j].pose.position.y;
                                unsigned int current_x_i, current_y_i;

                                if (!origin_costmap_->worldToMap(wx, wy, current_x_i, current_y_i)) {
                                    ROS_WARN_THROTTLE(1.0,
                                                      "errror 机器人的起始位置不在全球成本图上。计划总是会失败，你确定机器人已经正确本地化了吗 The goal sent to the global planner is off the global costmap. Planning will always fail to this goal.");
                                    response.message="errror 机器人的起始位置不在全球成本图上。计划总是会失败，你确定机器人已经正确本地化了吗 The goal sent to the global planner is off the global costmap. Planning will always fail to this goal.";
                                    return true;
                                }
                                Point point;
                                point.x=(double)current_x_i;
                                point.y=(double)current_y_i;
                                plan_points.push_back(point);


                            }
                            //end-----------makePlan------------------true 使用a*规划，或 使用d*规划；false 一阶贝塞尔-----------------

                            cout<<"9.****把所有半结构化路径在地图中的索引重新放在一个容器里akePlanWithSemiStructuredPlan.size(): "<<endl;

                        }else{


                            //1阶贝塞尔曲线规划
                            //start--------------FirstBezier plan---------------
                            int  distanceFirstBezier =sqrt(pow((int)morPlanToMap_startpoint.x - (int)morPlanToMap_startpoint.y, 2) + pow((int)target_goal_point.x - (int)target_goal_point.y, 2));
                            vector<double> ps;   //上一条路径的末端点
                            vector<double> pe;   //下一条路径的起点
                            ps.push_back(morPlanToMap_startpoint.x);
                            ps.push_back(morPlanToMap_startpoint.y);
                            pe.push_back(target_goal_point.x);
                            pe.push_back(target_goal_point.y);


                            ROS_INFO("callback_global_plannerFun 1阶贝塞尔曲线规划  distance is %i", distance);

                            //---
                            //1.封装一条路径【储存像素】，包括起点，终点，路径点，路径长度  [上一条路径的末端点 要到达的 下一条路径的起点]
                            // vector<Point> distance_plan;
                            //---
                            for (int l = 1; l < distanceFirstBezier; l++)
                            {
                                double t =(double)  l / (double) distanceFirstBezier;
                                //todo ************************** 一阶贝塞尔曲线计算插值 *********** ========================================================*****************
                                vector<double> result = FirstBezier(ps, pe,  t);
                                //---
                                //todo 2.封装一条路径【储存像素】，包括起点，终点，路径点，路径长度 [Point point(result[0],result[1]) ;distance_plan.push_back(point); ]  [计算两条线段交点处的圆弧路径使用]
                                Point point(result[0],result[1]);
                                //distance_plan.push_back(point);
                                plan_points.push_back(point);
                                //---
                                // vector<int> result = FirstBezier(ps, pe, t);
                                // geometry_msgs::PoseStamped pose;
                                double world_x, world_y;
                                //todo 像素坐标转世界坐标
                                mapToWorld(result[0], result[1], world_x, world_y);
                                geometry_msgs::PoseStamped pose;
                                pose.pose.orientation.x = 0.0;
                                pose.pose.orientation.y = 0.0;
                                pose.pose.orientation.z = 0.0;
                                pose.pose.orientation.w = 1.0;
                                //设置 geometry_msgs::PoseStamped数据保存到plan路径中【plan 已经是世界坐标了，上一行代码  mapToWorld(result[0], result[1], world_x, world_y); 像素坐标转世界坐标】
                                setPoseStampedPoseToPlan(pose,world_x,world_y, makePlanWithSemiStructuredPlan);

                            }
                            //end--------------FirstBezier---------------

                        }//end if(!ismakePlan)

                        //---------------------------------------------------
                        CurrentPathGoToTargetPath currentPathGoToTargetPath(plan_points,makePlanWithSemiStructuredPlan);
                        target_map_plan[target_plan_id]=currentPathGoToTargetPath;//target_plan_id=i 是对应路径的路径索引 index；每个路径在数据库中都会存储一个唯一的id
                        //打印数据
                        cout<<"==================可视化  marker_arc_pub_  上一条路径的末端点 要到达的 下一条路径的起点,链接成一条路径============================" << endl;
                        cout<<"可视化  marker_arc_pub_  父id： "<<current_plan_id<<"  ;子target_plan_id： "<< target_plan_id << endl;
                        cout<<"==============================================" << endl;

                        //todo 可视化
                        ros::Duration(0.2).sleep(); //todo 需要延时一下才会显示箭头
                        cout<<"==================   可视化  marker_arc_pub_ 把所有半结构化路径在地图中的索引重新放在一个容器里 ============================" << endl;
                        visualization_msgs::Marker marker;
                        //publishMarkerPlan(marker, path_with_rounded);// 出于可视化目的发布计划 MarkerPlan  visualization_msgs::Marker  ，int rgb 颜色 1：红，2：绿，3 蓝， ,doubl scale  0.1大小
                        publishMarkerPlan(marker, makePlanWithSemiStructuredPlan,3,0.06);// 出于可视化目的发布计划 MarkerPlan  visualization_msgs::Marker
                        marker_connect_pub_.publish(marker);//发布多路径 带圆弧

                    }//end for (int i = 0; i < target_plan_ids.size(); ++i)
                    //当前路径到达目标路径的路径
                    current_path_goTo_target_path_map_plan_[current_plan_id]=target_map_plan;
                    cout<< "11111 一条路径的终点会链接另一条路径的起点 current_path_goTo_target_path_map_plan_ 当前路径的 current_plan_id : "<< current_plan_id << endl;

                    //--------------------------------------------------------------------------------
                }// end for(iter = execute_map_plan_.begin(); iter != execute_map_plan_.end(); iter++)

                //-------------------------------原路返回会用到这个翻转的数据-----------------------------------------------
                 //翻转路径 当前路径到达目标路径的路径
                 ////数据翻转 遍历原有集合
                for (const auto& entry : current_path_goTo_target_path_map_plan_) {
                    int start_idx = entry.first;
                    const auto& sub_map = entry.second;

                    // 遍历二级子集合
                    for (const auto& sub_entry : sub_map) {
                        int end_idx = sub_entry.first;
                        const auto& path = sub_entry.second;

                        // 创建一个新的路径对象
                        CurrentPathGoToTargetPath new_path;

                        // 将 plan_points 反转排序
                        new_path.plan_points = vector<Point>(path.plan_points.rbegin(), path.plan_points.rend());

                        // 将 plan 中的 PoseStamped 反转排序
                        new_path.plan = vector<geometry_msgs::PoseStamped>(path.plan.rbegin(), path.plan.rend());

                        // 将起点和终点调换，并以新的方式存储
                        current_path_goTo_target_path_map_plan_Overturn_[end_idx][start_idx] = new_path;
                    }
                }

                //数据翻转
                execute_map_plan_Overturn_ = execute_map_plan_;
                map<int, std::vector<int>> new_target_plan_ids_map;
                for (auto &map_plan : execute_map_plan_Overturn_) {
                    new_target_plan_ids_map[map_plan.first] = map_plan.second.reverseData(execute_map_plan_Overturn_);
                }

                   // 更新每个元素的 target_plan_ids
                for (auto &map_plan : execute_map_plan_Overturn_) {
                    map_plan.second.target_plan_ids = new_target_plan_ids_map[map_plan.first];
                }

             //--------------------------------------------------------------------------







                //todo test 打印 debug 用的
                map<int, map<int, CurrentPathGoToTargetPath>>::iterator iter1; //迭代器 test
                for(iter1 = current_path_goTo_target_path_map_plan_.begin(); iter1 != current_path_goTo_target_path_map_plan_.end(); iter1++) {
                    int current_plan_id1 = iter1->first; //当前路径id，唯一id ；每个路径在数据库中都会存储一个唯一的id
                    cout<< "222 test 一条路径的终点会链接另一条路径的起点 current_path_goTo_target_path_map_plan_ 当前路径的父亲 current_plan_id1 : "<< current_plan_id1 << endl;
                    map<int, CurrentPathGoToTargetPath>::iterator iter2; //迭代器 test
                    map<int, CurrentPathGoToTargetPath> target_map_plan =   current_path_goTo_target_path_map_plan_[current_plan_id1];
                    for(iter2 = target_map_plan.begin(); iter2 != target_map_plan.end(); iter2++) {
                        int current_plan_id2 = iter2->first; //当前路径id，唯一id ；每个路径在数据库中都会存储一个唯一的id
                        cout<< "333 test 一条路径的终点会链接另一条路径的起点 target_map_plan 当前路径的子 id current_plan_id2 : "<< current_plan_id2 << endl;

                    }
                }

//                if(state_machine_.get_current_state() == "init_global_waypoints"){
//                    //状态机
//                    state_machine_.set_state("init_global_path", {});//1.选择点位设置全局路径 init_global_waypoints
//                }else{
//                       ROS_ERROR("navigate_to_goal 发送导航失败，情等待状态切换为闲置状态 idle 才能继续导航操作。当前状态Invalid state: %s", state_machine_.get_current_state().c_str() );
//                   }

                //-------------------------------------------------------------------------------------------------

                //-------------------------
            }else  if(request.waypointAction==request.GETPLAN){// GETPLAN 获取路径
                std::cout << "1.============================================= GETPLAN 获取路径 ==========================================================" << state_machine_.get_current_state()<< std::endl;

                //1.等待数据准备完毕
                std::unique_lock<std::mutex> lock(data_mutex);
                std::cout << "2.============================================= GETPLAN 获取路径 ==========================================================" << state_machine_.get_current_state()<< std::endl;

                isOneWay_=  request.isOneWay;//判断能不能倒着走,是否单向导航 true 单向导航，false 双向导航

                //todo (在这里不需要 request_default_goal 了 2023 05 19 )
                request.request_default_goal.header.stamp = ros::Time::now();
                request.request_default_goal.header.frame_id = frame_id_;
                request.request_default_goal.pose.position.x = -4.4;
                request.request_default_goal.pose.position.y = 1.11;
                request.request_default_goal.pose.position.z = 0.0;
                request.request_default_goal.pose.orientation.x = 0.0;
                request.request_default_goal.pose.orientation.y = 0.0;
                request.request_default_goal.pose.orientation.z = 0.0;
                request.request_default_goal.pose.orientation.w = 1.0;

//                if(request.request_default_goal.header.frame_id==""){
//                    string meg="request.request_default_goal.header.frame_id==map";
//                    ROS_INFO("默认点位，或是充电点位 必须给值 request.request_default_goal  frame_id_ %s ",frame_id_.c_str());
//                    response.message="errror 默认点位，或是充电点位 必须给值 request.request_default_goal frame_id_: %s "+frame_id_;
//                    data_ready = false;//恢复数据，用互斥锁保证数据的安全
//                    //日志
//                    contnav_error(__FILE__,__LINE__,const_cast<char*>(response.message.c_str()));
//                    return true;
//                }

//                //返回默认点位，或是充电点位
//                target_pose_default_=request.request_default_goal;
//                response.message="GETPLAN 获取路径 ";
//
//
//                //判断这个数据是否有数据
//                if (target_pose_default_.header.frame_id == "") {
//                    response.message="error GETPLAN 获取路径,没有收到默认路径点位 request_default_goal; ";
//                    response.result = ServiceResultType::ResultTypeERROR;
//                    data_ready = false;//恢复数据，用互斥锁保证数据的安全
//                    //日志
//                    contnav_error(__FILE__,__LINE__,const_cast<char*>(response.message.c_str()));
//                    return true;
//                }



                //开始坐标
                geometry_msgs::PoseStamped start_pose;
                if(!getRobotPose(start_pose)){
                    ROS_ERROR("my_global_planner cannot make a plan for you because it could not get the start pose of the robot");
                    //日志
                    //  contnav_error(__FILE__,__LINE__,const_cast<char*>(response.message.c_str()));
                    //  contnav_error(__FILE__,__LINE__,"global_planner::goalCB 获取小车当前位置 error");
                    response.message="get robotpose error";
                    response.result = ServiceResultType::ResultTypeERROR;
                    data_ready = false;//恢复数据，用互斥锁保证数据的安全
                    //日志
                    contnav_error(__FILE__,__LINE__,const_cast<char*>(response.message.c_str()));
                    return true;
                }

                //目标坐标
                request.request_goal.header.stamp = ros::Time::now();
                request.request_goal.header.frame_id = frame_id_;
                // todo 测试数据
//                request.request_goal.pose.position.x = -0.876;
//                request.request_goal.pose.position.y = 1.0;
//                request.request_goal.pose.position.z = 0.0;
//                request.request_goal.pose.orientation.x = 0.0;
//                request.request_goal.pose.orientation.y = 0.0;
//                request.request_goal.pose.orientation.z = 0.0;
//                request.request_goal.pose.orientation.w = 1.0;

                //默认点位
                //target_pose_default_


                if(!request.isSendGoalPlanOverturn){//  是否反转路径导航，true 反转路径导航，false 正常路径导航
                    std::vector<geometry_msgs::PoseStamped> plan_bendi;
                    int state_params_size1=state_machine_.set_state("navigate_to_goalpath", {start_pose,request.request_goal, request.request_default_goal,std::string("EMPTY"),plan_bendi});// target_pose_default_;//返回默认点位，或是充电点位
                    std::cout << "1.============================================= GETPLAN 获取路径 等待数据准备完毕===============navigate_to_goalpath===========================================" << state_machine_.get_current_state()<< std::endl;

                }else{
                    std::vector<geometry_msgs::PoseStamped> plan_bendi;
                    //切换到翻转路径
                    int state_params_size2=state_machine_.set_state("navigate_to_goalpathOverturn", {start_pose,request.request_goal, request.request_default_goal,std::string("EMPTY"),plan_bendi});//
                    std::cout << "2.============================================= GETPLAN 获取路径 等待数据准备完毕===============navigate_to_goalpathOverturn===========================================" << state_machine_.get_current_state()<< std::endl;


                }
                std::cout << "3.============================================= GETPLAN 获取路径 等待数据准备完毕===============data_cv.wait(lock, []{ return data_ready; })===========================================" << state_machine_.get_current_state()<< std::endl;

                //todo ==============================================================================================2. data_cv.wait 等待数据准备完毕=======================================================================================
                data_ready = false;  // Reset the flag before waiting.
                data_cv.wait(lock, []{ return data_ready; });
                std::cout << "4.============================================= GETPLAN 获取路径 等待数据准备完毕===============data_cv.wait(lock, []{ return data_ready; })===========================================" << state_machine_.get_current_state()<< std::endl;


                //todo ==============================================================================================1.得到数据=======================================================================================

                try {

                    //获取状态机获取
                    std::vector<geometry_msgs::PoseStamped> result_plan;//得到最终的路径
                    boost::any age3,age4;
                    state_machine_.get_current_state_param(3,age3);//获取当前状态的参数
                    state_machine_.get_current_state_param(4,age4);//获取当前状态的参数
                    std::string extra_string  = any_cast<std::string>(age3);
                    result_plan = any_cast<std::vector<geometry_msgs::PoseStamped>>(age4);//todo 得到最终的路径
                    std::cout << "5.============================================= GETPLAN 获取路径 等待数据准备完毕===============data_cv.wait(lock, []{ return data_ready; })===========================================" << state_machine_.get_current_state()<<std::endl;

                    // 获取当前状态的成员函数
                    std::string  current_state = state_machine_.get_current_state();
                    ROS_INFO_STREAM("2.callback_global_plannerFun 获取路径 创建Twist消息并设置线速度和角速度Type of age4: " << age4.type().name());
                    std::cout << "2. callback_global_plannerFun获取路径:current_state " << current_state << std::endl;
                    if(state_machine_.get_current_state()=="monitor_progress" && extra_string=="success"){
                        response.message="GETPLAN 获取路径,路径规划成功";
                        response.result = ServiceResultType::ResultTypeOK;
                        response.result_plan=result_plan;//todo 得到最终的路径
                        //日志
                        contnav_info(__FILE__,__LINE__,const_cast<char*>(response.message.c_str()));
                    }else{
                        //日志
                        //  contnav_error(__FILE__,__LINE__,const_cast<char*>(response.message.c_str()));
                        //  contnav_error(__FILE__,__LINE__,"global_planner::goalCB 获取小车当前位置 error");
                        response.message="GETPLAN 获取路径,路径规划失败";
                        response.result = ServiceResultType::ResultTypeERROR;
                        //日志
                        contnav_error(__FILE__,__LINE__,const_cast<char*>(response.message.c_str()));
                    }



                } catch (const std::exception& e) {
                    ROS_ERROR("exception 切换到翻转路径获取路径 callback_global_plannerFun(): %s", e.what());
                    //日志
                    contnav_error(__FILE__,__LINE__,const_cast<char*>("1.exception 切换到翻转路径获取路径 callback_global_plannerFun()"));
                } catch (...) {
                    ROS_ERROR("exception 切换到翻转路径获取路径 callback_global_plannerFun()");
                    //日志
                    contnav_error(__FILE__,__LINE__,const_cast<char*>("2.exception 切换到翻转路径获取路径 callback_global_plannerFun()"));
                }
                std::cout << "6.============================================= GETPLAN 获取路径 等待数据准备完毕===============data_cv.wait(lock, []{ return data_ready; })===========================================" << std::endl;

                //初始化给空闲状态
                std::vector<geometry_msgs::PoseStamped> plan_bendi;
                state_machine_.set_state("idle", {start_pose,request.request_goal,request.request_default_goal,std::string("EMPTY"),plan_bendi});//闲置状态,初始化给一个控制
                std::cout << "7.2 callback_global_plannerFun获取路径 初始化给空闲状态 : current_state " << state_machine_.get_current_state() << std::endl;
                std::cout << "7.end============================================= GETPLAN 获取路径 等待数据准备完毕===============data_cv.wait(lock, []{ return data_ready; })===========================================" << std::endl;

            }else  if(request.waypointAction==request.DELETE){//删除上一次增加的点
                response.message="删除上一次增加的点 ";
                //删除最后一个增加的数据
                if(waypoints_.size()>0)
                    waypoints_.pop_back();
            }else  if(request.waypointAction==request.DELETEALL){//删除所有队列中的点
                response.message="删除所有队列中的点 ";
                if(waypoints_.size()>0)
                    waypoints_.clear();//清除数据
            }else  if(request.waypointAction==request.DELETEMARKER){//  DELETEMARKER 删除指定命名空间与id的一条marker路径
                response.message="删除上一条marker路径 ";
                visualization_msgs::Marker marker;
                marker.header.frame_id = frame_id_;
                marker.header.stamp = ros::Time::now();
                marker.ns = "publishMarkerPlan";//以后改成当前地图名称，从数据库读取
                marker.id = 1;//对应的id
                // %Tag(ACTION)%
                marker.action = visualization_msgs::Marker::DELETE;
                marker_pub_.publish(marker);
            }else  if(request.waypointAction==request.DELETEALLARKER){//  DELETEALLARKER 删除所有marker路径
                response.message="删除所有marker路径 ";
                visualization_msgs::Marker marker;
                marker.header.frame_id = frame_id_;
                marker.header.stamp = ros::Time::now();
                marker.action = visualization_msgs::Marker::DELETEALL;
                marker_pub_.publish(marker);
            }else  if(request.waypointAction==request.DELETEMORPLANTOMAP){//清空 execute_plan_容器中所有需要执行的路径
                response.message="清空 execute_plan_容器中所有需要执行的路径！";
                init();

            }





        }//if   else if(request.request_type=="request.waypoint")
        else if(request.request_type=="readpose") {//读取txt中的path plan数据

            try{

                if(isSavePlanToTxt_){
                    std::string filename = fileOutPut_+"/planpose/plan.txt";
                    std::vector<geometry_msgs::PoseStamped> plan = readEulerAnglesFromFile(filename);
                    if (plan.empty())
                    {
                        std::cerr << "读取txt中的path plan数据Failed to read data from file: " << filename << std::endl;
                        std::string filenameMessage = "读取txt中的path plan数据Failed to read data from file: "+filename;
                        contnav_error(__FILE__,__LINE__,const_cast<char*>(filenameMessage.c_str()));

                        return 1;
                    }

                    std::cout << " 读取txt中的path plan数据 Successfully read " << plan.size() << " poses from file: " << filename << std::endl;
                    std::string filenameMessage = "读取txt中的path plan数据 Successfully read,poses from file: "+filename;

                    response.message=filenameMessage;
                    response.result = ServiceResultType::ResultTypeOK;
                    response.result_plan=plan;//todo 得到最终的路径
                    //日志
                    contnav_info(__FILE__,__LINE__,const_cast<char*>(response.message.c_str()));


                    nav_msgs::Path gui_path;
                    publishPlan(plan,gui_path);
                    pathPlan_semiStructured_pub_.publish(gui_path);
                }else{
                    //+" ,isSendGoalPlanOverturn: "+(request.isSendGoalPlanOverturn?"true" : "false")
                    std::string filenameMessage = "读取txt中的path plan数据 Failed to read data from file,请确保 isSavePlanToTxt_参数为true，路径已经记录到 plan.txt文件中， isSavePlanToTxt_： "+isSavePlanToTxt_?"true":"false";
                    contnav_error(__FILE__,__LINE__,const_cast<char*>(filenameMessage.c_str()));
                }

            }catch (std::exception e){
                std::string filenameMessage = "exception 读取txt中的path plan数据Failed to read data from file: ";
                contnav_error(__FILE__,__LINE__,const_cast<char*>(filenameMessage.c_str()));
            }



        }//else if(request.request_type=="readpose")
        else if(request.request_type=="clear_costmaps") {
              //清空代价地图
            std::cout << " 清空代价地图 "  << std::endl;
            //恢复代价地图
            std_srvs::Empty srv;
            clear_costmaps_client_.call(srv);
            contnav_info(__FILE__,__LINE__,const_cast<char*>(" 清空代价地图 "));

        }//else if(request.request_type=="clear_costmaps")

        else if(request.request_type=="getExecuteMapPlanSize") {


            std::cout << "1.路径规划 getExecuteMapPlanSize " <<request.request_type<<" : " <<request.makePlan<<std::endl;
            try{
                response.result_executePlanSize=execute_map_plan_.size();
                std::cout << "2.路径规划 getExecuteMapPlanSize " <<request.request_type<<" : " <<request.makePlan<<std::endl;

                if(execute_map_plan_.size()>0) {
                    std::cout << "3.路径规划 getExecuteMapPlanSize " <<request.request_type<<" : " <<request.makePlan<<std::endl;

                    response.message="存在execute_map_plan_ ";
                    response.result = ServiceResultType::ResultTypeOK;
                    fig=true;
                }else{
                    std::cout << "4.路径规划 getExecuteMapPlanSize " <<request.request_type<<" : " <<request.makePlan<<std::endl;

                    response.message="不存在execute_map_plan_ ";
                    response.result = ServiceResultType::ResultTypeERROR;
                    fig=false;
                }
            }catch (exception e){
                std::cout << "5.路径规划 getExecuteMapPlanSize " <<request.request_type<<" : " <<request.makePlan<<std::endl;

                response.message="不存在execute_map_plan_ ,异常 exception: ";
                response.result = ServiceResultType::ResultTypeERROR;

                contnav_error(__FILE__,__LINE__,const_cast<char*>("不存在execute_map_plan_ ,异常 exception"));




                fig=false;
            }

            std::cout << "6.路径规划 getExecuteMapPlanSize " <<request.request_type<<" : " <<request.makePlan<<std::endl;

        }


        //end-------------------------获取rviz登界面上取的点，或者从数据库取点 规划路径---------------------------------------
        std::cout << "8.路径规划 ================================================== " << std::endl;


        } catch (std::exception e) {
            //互锁
           // m_.unlock();
            cout << e.what() << endl;
            ROS_ERROR("callback_global_plannerFun()->exception ");
            contnav_error(__FILE__,__LINE__,const_cast<char*>("callback_global_plannerFun()->exception "));

        }
        //延时
//        ros::Rate r((0.2));
//        r.sleep();
        //互锁
       // m_.unlock();
        if (fig) {
            response.result = ServiceResultType::ResultTypeOK;
            std::cout << "9.路径规划 =====================ServiceResultType::ResultTypeOK============================= " << std::endl;

        }else{
            response.result = ServiceResultType::ResultTypeERROR;
            std::cout << "10.路径规划 ============ServiceResultType::ResultTypeERROR========callback_global_plannerFun============================== " << std::endl;

        }
        //    std::cout << "====request.sqlStr==== "<< request.sqlStr << std::endl;
        std::cout << "11.路径规划 end=====================callback_global_plannerFun============================= " << std::endl;

        return true;
    }

    //todo 半结构路径规化 研发步骤
    /**
        1.获取路径规划的起点和终点
          1.1 找到路径规划的起点和终点的在结构化路径中的代价直最近的点，找到路径规划的起点和终点的在结构化路径中的代价直最近的点的下标
              1.1.1 初始化execute_map_potential，设置为无穷大，遍历所有结构化路径的点，放入 float* execute_map_potential 容器中，给一个初始代价直
              1.1.1 把路径规划的起点和终点的在结构化路径中的代价直最近的点的下标，放入到一个容器中


        2.获取路径规划的起点和终点的方向
        3.获取路径规划的起点和终点的距离
        4.获取路径规划的起点和终点的距离的方向
        5.获取路径规划的起点和终点的距离的方向的角度
        6.获取路径规划的起点和终点的距离的方向的角度的弧度

     */




    //这里需要找到最近的结构化网格点
    bool   global_planner::find_nearest_index(unsigned char* costs,int &start_index,int  &nearest_index){
        bool  nearest_indexFig = false;
        if (start_index < 0 || start_index >= ns_)//如果下标越界，则直接返回。  ns_ = nx * ny;
            return nearest_indexFig;


        return nearest_indexFig;
    }

    //todo 判断起始点在不在结构化网格上，int &index：地图数据中的索引值
    bool global_planner::is_index_in_planFun(int &find_index,std::vector<int> &plan_all_indexs) {
        bool is_index_in_plan = false;
        if (std::find(plan_all_indexs.begin(), plan_all_indexs.end(), find_index) != plan_all_indexs.end()) {//plan_all_indexs_ 当前所有半机构化网格路径在地图中的所有索引
            std::cout << "/+++++++++++++++///2: 已经在列表中了     :" << std::to_string(find_index) << std::endl;
            is_index_in_plan = true;
        }
        return is_index_in_plan;
    }

    //todo 判断起始点在不在结构化网格上，int &index：地图数据中的索引值
    bool global_planner::is_index_in_planFun(int &find_index,std::vector<int> &plan_all_indexs,int &result_i) {
        bool is_index_in_plan = false;
        //index：地图数据中的索引值
        vector<int>::iterator num = find(plan_all_indexs.begin(), plan_all_indexs.end(), find_index);  //返回一个迭代器指针
        if (num != plan_all_indexs.end())   //查找成功
        {
            cout << "地图数据中的索引值元素 " << *num << " 在 plan_all_indexs 中"<<endl;   //注意指针的输出方式
            cout << "其在类表中的索引为 " << distance(plan_all_indexs.begin(), num) << endl;
            result_i= distance(plan_all_indexs.begin(), num);//,result_i 返回的列表索引
            //distance()函数用于计算两个迭代器表示的范围内包含元素的个数
            is_index_in_plan = true;
        }
        else{
            cout<< "元素 " << find_index << " 不在num_list中" << endl;
        }

        return is_index_in_plan;
    }

    //todo 发布路径规划的marker，用于可视化
    bool   global_planner::publishPlanMarkerFun(visualization_msgs::Marker &marker,std::vector<geometry_msgs::PoseStamped> &plan,nav_msgs::Path &gui_path){
        if(plan.size()>2){

            //todo 路径规划中，一条路径的拐角处改成圆弧路径,圆弧半径自适应 （失败了）。
            //std::vector<geometry_msgs::PoseStamped> path_with_rounded=  path_with_rounded_corners(plan);
            //ROS_ERROR("path_with_rounded.size()i %i",path_with_rounded.size());
            //publish the plan for visualization purposes 出于可视化目的发布计划
            //std::vector<geometry_msgs::PoseStamped> plan; //【plan 已经是世界坐标了】
            //publishPlan(path_with_rounded,gui_path);//出于可视化目的发布计划，todo 这里可以注释掉，不需要显示路径
            publishPlan(plan,gui_path);//出于可视化目的发布计划，todo 这里可以注释掉，不需要显示路径

            //visualization_msgs::Marker marker;
            plan.size();
            if(plan.size()%2!=0){//marker 数据个数必须是偶数
                plan.push_back(plan[plan.size()-1]);
            }
            //publishMarkerPlan(marker, path_with_rounded);// // 出于可视化目的发布计划 MarkerPlan  visualization_msgs::Marker  ，int rgb 颜色 1：红，2：绿，3 蓝， ,doubl scale  0.1大小
            publishMarkerPlan(marker, plan,2,0.1);// 出于可视化目的发布计划 MarkerPlan  visualization_msgs::Marker
           // plan_pub.publish(gui_path);
           // marker_pub.publish(marker);
            ROS_ERROR("publishPlanMarkerFun gui_path.poses.size()i %i",gui_path.poses.size());
            ROS_ERROR("publishPlanMarkerFun plan.size()i %i",plan.size());
            //ROS_ERROR("path_with_rounded.size()i %i",path_with_rounded.size());
            ROS_INFO("publishPlanMarkerFun 获取直线 路径成功");

            return true;
        }else{
            ROS_ERROR("publishPlanMarkerFun获取直线 路径失败 ");
            std::cout << "publishPlanMarkerFun获取直线 路径失败  " << std::endl;

            return false;
        }

    }




    //通过起始点和结束点获取一条 贝赛尔直线 路径; startPoseStamped:起始点 endPoseStamped:结束点 plan:路径 (double)
    bool  global_planner::getline(geometry_msgs::PoseStamped &startPoseStamped,geometry_msgs::PoseStamped &endPoseStamped,std::vector<geometry_msgs::PoseStamped> &plan,nav_msgs::Path &gui_path){
        vector<double> ps;   //起始点坐标
        vector<double> pe;   //终点坐标
        //todo 1.起始点坐标世界坐标转像素坐标 [需要计算点数及t值]
        unsigned int world_i_start_x, world_i_start_y;//起始点坐标（像素）
        //世界坐标转成像素坐标
        if (!origin_costmap_->worldToMap(startPoseStamped.pose.position.x, startPoseStamped.pose.position.y, world_i_start_x, world_i_start_y)){
            ROS_ERROR("世界坐标转成像素坐标");
            return false;
        }
        ps.push_back((double)world_i_start_x);
        ps.push_back((double)world_i_start_y);
        //todo 2.目标坐标 （像素）
        unsigned int world_i_goal_x, world_i_goal_y;
        //世界坐标转像素坐标
        if (! origin_costmap_->worldToMap(endPoseStamped.pose.position.x, endPoseStamped.pose.position.y, world_i_goal_x, world_i_goal_y)){
            ROS_ERROR("世界坐标转成像素坐标");
            return false;
        }
        pe.push_back((double)world_i_goal_x);
        pe.push_back((double)world_i_goal_y);


        //计算路径的朝向，计算向量的斜率，向量的朝向 方向
        double angle;
        tf2::Quaternion q=getSlopeOfVector(startPoseStamped.pose.position.x, startPoseStamped.pose.position.y,endPoseStamped.pose.position.x, endPoseStamped.pose.position.y,angle);

        //todo 3.计算贝塞尔曲线的点数及t值，先计算两点之间的距离，然后根据距离计算点数，然后计算t值
        // 通过开始点与控制点计算两点直接的像素直合宿据 Calculate the distance between two points using the Pythagorean theorem
        int  distance =sqrt(pow((int)world_i_goal_x - (int)world_i_start_x, 2) + pow((int)world_i_goal_y - (int)world_i_start_y, 2));
        ROS_INFO("getline 通过开始点与控制点计算两点直接的像素直 distance is %i", distance);
        //---
        //1.封装一条路径【储存像素】，包括起点，终点，路径点，路径长度  [计算两条线段交点处的圆弧路径使用]
        vector<Point> distance_plan;
        //---
        for (int i = 1; i < distance; i++)
        {
            double t =(double)  i / (double) distance;
            //todo ************************** 一阶贝塞尔曲线计算插值 *********** ========================================================*****************
            vector<double> result = FirstBezier(ps, pe,  t);
            //---
             //todo 2.封装一条路径【储存像素】，包括起点，终点，路径点，路径长度 [Point point(result[0],result[1]) ;distance_plan.push_back(point); ]  [计算两条线段交点处的圆弧路径使用]
            Point point(result[0],result[1]);
            distance_plan.push_back(point);
            //---
            // vector<int> result = FirstBezier(ps, pe, t);
            // geometry_msgs::PoseStamped pose;
            double world_x, world_y;
            //todo 像素坐标转世界坐标
            mapToWorld(result[0], result[1], world_x, world_y);
            geometry_msgs::PoseStamped pose;
            pose.pose.orientation.x = q.x();
            pose.pose.orientation.y = q.y();
            pose.pose.orientation.z = q.z();
            pose.pose.orientation.w = q.w();
            //设置 geometry_msgs::PoseStamped数据保存到plan路径中【plan 已经是世界坐标了，上一行代码  mapToWorld(result[0], result[1], world_x, world_y); 像素坐标转世界坐标】
            setPoseStampedPoseToPlan(pose,world_x,world_y, plan);

        }
        //---
         //3.封装一条路径【储存像素】，包括起点，终点，路径点，路径长度 [Point point(result[0],result[1]) ;distance_plan.push_back(point); ]  [计算两条线段交点处的圆弧路径使用]
        OnePlanToMap onePlanToMap(world_i_start_x,world_i_start_y,world_i_goal_x,world_i_goal_y,distance,distance_plan);
        //4.封装一条路径【储存像素】，包括起点，终点，路径点，路径长度 [Point point(result[0],result[1]) ;distance_plan.push_back(point); ]  [计算两条线段交点处的圆弧路径使用]
        onePlanToMaps_vector_.push_back(onePlanToMap);//todo [计算两条线段交点处的圆弧路径使用]
        //---
        ROS_ERROR("plan.size()  %i",plan.size());

        if(plan.size()>2){
            // add orientations if needed 如果需要，添加方向
            orientation_filter_->processPath(startPoseStamped, plan);


            //publish the plan for visualization purposes 出于可视化目的发布计划
            //std::vector<geometry_msgs::PoseStamped> plan;
            //   publishPlan(plan,gui_path);//出于可视化目的发布计划，todo 这里可以注释掉，不需要显示路径
            ROS_INFO("getline 获取直线 路径成功");
            return true;
        }else{
            ROS_ERROR("getline 获取直线 路径规划失败");
            std::cout << "getline 获取直线 路径失败  " << std::endl;
            return false;
        }

    }
     //todo 计算圆弧路径 ； 函数中 调用全局容器 onePlanToMaps_vector_ 获取路径数据；【onePlanToMaps_vector_数据在 函数 getline中进行封装】,&distance_plan_all_vector 保存所有路径点 包含圆弧的路径点,vector<Point>  &distance_plan_noArc_vector 保存所有包含转角的路径点
    bool global_planner::global_planner_path_with_rounded_corners(geometry_msgs::PoseStamped &startPoseStamped,std::vector<geometry_msgs::PoseStamped> &plan,vector<Point>  &distance_plan_all_vector,vector<Point>  &distance_plan_noArc_vector) {
         ROS_ERROR("1.计算圆弧路径 onePlanToMaps_vector_.size() %i",onePlanToMaps_vector_.size());
        if(onePlanToMaps_vector_.size()<=0){
            ROS_ERROR("onePlanToMaps_vector_ 为空!");

            return false;
        }

       //  vector<Point>  distance_plan_all_vector;//todo 保存所有路径点 包含圆弧的路径点
        // vector<Point>  distance_plan_noArc_vector;//todo 保存所有包含转角的路径点
         vector<Point>  distance_plan_temporary_vector;//todo 保存临时路径点

         //todo 1.------------------------第1条路径-------------------------------
         OnePlanToMap onePlanToMap = onePlanToMaps_vector_[0];//todo 第1条路径
         //todo 1.起始点坐标世界坐标转像素坐标 [需要计算点数及t值]
         int world_i_start_x =  onePlanToMap.world_i_start_x;
         int world_i_start_y =  onePlanToMap.world_i_start_y;
         //todo 2.目标坐标 （像素）
         int world_i_goal_x =  onePlanToMap.world_i_goal_x;
         int world_i_goal_y =  onePlanToMap.world_i_goal_y;
         // 两点之间的线段长度（像素）
         int distance =  onePlanToMap.distance;
         ////路径点坐标（像素）[一阶贝塞尔曲线计算出来的两点的直线像素点坐标数据]
         vector<Point>  distance_planFirst =  onePlanToMap.distance_plan;//Point double x,y;
         //第1条路径
         for (int j = 0; j < distance_planFirst.size(); ++j) {
             distance_plan_all_vector.push_back(distance_planFirst[j]);//todo 第1条路径保存到所有路径点
             distance_plan_noArc_vector.push_back(distance_planFirst[j]);//todo 第1条保存所有包含转角的路径点
            // int index = toIndex((int)distance_planFirst[j].x, (int)distance_planFirst[j].y,nx_,ny_);//将周围点的坐标转化为数组下标。 toIndex(int x, int y,int nx ,int ny)
         }

         ROS_ERROR("2.计算圆弧路径 第1条路径 distance_planFirst.size() %i",distance_planFirst.size());
         //---直线路径
         if(onePlanToMaps_vector_.size()==1){
             ROS_ERROR("onePlanToMaps_vector_ 只有一个节点，不需要做圆弧路径");
             //遍历所有路径
             for (int i = 0; i < distance_plan_all_vector.size() ; ++i) {
                 double world_x, world_y;
                 //todo 像素坐标转世界坐标
                 mapToWorld(distance_plan_all_vector[i].x, distance_plan_all_vector[i].y, world_x, world_y);
                 geometry_msgs::PoseStamped pose;
                 pose.pose.orientation.x = 0.0;
                 pose.pose.orientation.y = 0.0;
                 pose.pose.orientation.z = 0.0;
                 pose.pose.orientation.w = 1.0;
                 //设置 geometry_msgs::PoseStamped数据保存到plan路径中【plan 已经是世界坐标了，上一行代码  mapToWorld(result[0], result[1], world_x, world_y); 像素坐标转世界坐标】
                 setPoseStampedPoseToPlan(pose,world_x,world_y, plan);
             }
             if (plan.size() > 0) {
                 ROS_INFO("Found global plan");
                 // add orientations if needed 如果需要，添加方向
                 orientation_filter_->processPath(startPoseStamped, plan);
                 ROS_INFO(" 圆弧 路径成功");
                 return true;
             } else {
                 ROS_ERROR("Failed to find global plan");
                 return false;
             }
         }


         //---

         ROS_ERROR("3.计算圆弧路径 遍历各节点的数据，各路径线段的数据， onePlanToMaps_vector_.size() %i",onePlanToMaps_vector_.size());
         for (int i = 0; i < (onePlanToMaps_vector_.size()-1); ++i) {

             //todo 1.--------------todo 保存所有包含转角的路径点--------------------------------------
             OnePlanToMap onePlanToMap_noarc = onePlanToMaps_vector_[i+1];
             vector<Point>  distance_plan_noarc =  onePlanToMap_noarc.distance_plan;
             for (int j = 0; j < distance_plan_noarc.size(); ++j) {
                 distance_plan_noArc_vector.push_back(distance_plan_noarc[j]);//todo 保存所有包含转角的路径点
             }

             //------------------------------开始圆弧计算--------------------------------------
             //todo 1.------------------------第1条路径-------------------------------
             //todo 1.起始点坐标世界坐标转像素坐标 [需要计算点数及t值]
             int world_i_start_x =  onePlanToMap.world_i_start_x;
             int world_i_start_y =  onePlanToMap.world_i_start_y;
             //todo 2.目标坐标 （像素）
             int world_i_goal_x =  onePlanToMap.world_i_goal_x;
             int world_i_goal_y =  onePlanToMap.world_i_goal_y;
             // 两点之间的线段长度（像素）
             int distance =  onePlanToMap.distance;
             ////路径点坐标（像素）[一阶贝塞尔曲线计算出来的两点的直线像素点坐标数据]
             vector<Point>  distance_plan =  onePlanToMap.distance_plan;

             //todo 2.----------------------第2条路径---------------------------------
             OnePlanToMap onePlanToMap_NEX = onePlanToMaps_vector_[i+1];//todo 第2条路径
             //todo 1.起始点坐标世界坐标转像素坐标 [需要计算点数及t值]
             int world_i_start_x_NEX =  onePlanToMap_NEX.world_i_start_x;
             int world_i_start_y_NEX =  onePlanToMap_NEX.world_i_start_y;
             //todo 2.目标坐标 （像素）
             int world_i_goal_x_NEX =  onePlanToMap_NEX.world_i_goal_x;
             int world_i_goal_y_NEX =  onePlanToMap_NEX.world_i_goal_y;
             // 两点之间的线段长度（像素）
             int distance_NEX =  onePlanToMap_NEX.distance;
             ////路径点坐标（像素）[一阶贝塞尔曲线计算出来的两点的直线像素点坐标数据]
             vector<Point>  distance_plan_NEX =  onePlanToMap_NEX.distance_plan;
             //todo 3.----------------计算两条线段交点处的圆弧路径-----------------------------------
             //3.用两条路径的起始点和终点，计算两条线段交点处的圆弧路径，第一條路徑的終點和第二條路徑的起點是同一个交点都可以作为二阶贝塞尔曲线的控制点
             vector<double> pc;//todo pc 贝塞尔pc 控制点坐标 (第一條路徑的終點和第二條路徑的起點是同一个交点，都可以作为二阶贝塞尔曲线的控制点)
             pc.push_back((double)world_i_start_x_NEX);
             pc.push_back((double)world_i_start_y_NEX);
             ROS_ERROR("4.计算圆弧路径 贝塞尔pc 控制点坐标 ， pc.x %d : pc.y %d",pc[0],pc[1]);

            int distance_plan_min_size = std::min(distance_plan.size(), distance_plan_NEX.size());//取最小的一个直线路径点数
            int point_size_t=0;//绘制点数
            if((onePlanToMaps_vector_.size()==2&&distance_plan_min_size>point_size_t_)
             ||(onePlanToMaps_vector_.size()>2&&(distance_plan_min_size>point_size_t_*2))){//(onePlanToMaps_vector_.size()>2&&(distance_plan_size>point_size_t_*2) //中间的线段需要首位作圆弧，所以是线段需要要2倍的点
                point_size_t=point_size_t_;
            }else if((onePlanToMaps_vector_.size()==2&&distance_plan_min_size>point_min_size_t_)
                     ||(onePlanToMaps_vector_.size()>2&&(distance_plan_min_size>point_min_size_t_*2))){
                point_size_t=point_min_size_t_;
            }else {
                point_size_t=point_min_size_t_/2;//取最小的一个直线路径点数的一半
            }

            //（第一条线段与第二条线段首尾相连，所以第一条从后面开始数，第二条从前面开始数）
             vector<double> ps;//todo ps 贝塞尔 点ps
             //取第一条现段 ps 点索引，从线段尾部开始取
             int distance_plan_index= distance_plan.size()-point_size_t;
             Point distance_plan_point=  distance_plan[distance_plan_index];
             ps.push_back((double)distance_plan_point.x);
             ps.push_back((double)distance_plan_point.y);

             ROS_ERROR("5.计算圆弧路径 ps 贝塞尔 点Ps， ps.x %d : ps.y %d",ps[0],ps[1]);

             //---
             vector<double> pe;//todo pe 贝塞尔 点 pe
             //取第二条现段 ps 点索引，从线段前部开始取
             int distance_plan_NEX_index= point_size_t;
             Point distance_plan_NEX_point=  distance_plan_NEX[distance_plan_NEX_index];
             pe.push_back((double)distance_plan_NEX_point.x);
             pe.push_back((double)distance_plan_NEX_point.y);
             ROS_ERROR("6.计算圆弧路径 pe 贝塞尔 点Pe， pe.x %d : pe.y %d",pe[0],pe[1]);
             //---
             ROS_ERROR("7.计算圆弧路径  point_size_t 个点 %i",point_size_t);
             vector<Point> arc_point;//圆弧路径点坐标
             //二阶贝塞尔计算圆弧  point_size_t 个点
             for (int i = 1; i < point_size_t; i++)
             {
                 double t =(double)  i / (double) point_size_t;
                 //二阶贝塞尔曲线
                 vector<double> result = QuadraticBezier(ps, pc, pe, t);
                 //----圆弧路径点坐标
                 Point point_arc(result[0],result[1]);
                 arc_point.push_back(point_arc);
               //todo 4.----------------计算圆弧路径-----------------------------------
             }

             ROS_ERROR("8.计算圆弧路径  圆弧路径点坐标个点arc_point.size() %i",arc_point.size());

             ROS_ERROR("9.计算圆弧路径   point_size_t 个点 point_size_t %i",point_size_t);
             ROS_ERROR("10.计算圆弧路径  上条路径 distance_plan_all_vector.size() %i",distance_plan_all_vector.size());

             //上条路径
             for (int j = 0; j < (distance_plan_all_vector.size()-point_size_t); ++j) {

                 distance_plan_temporary_vector.push_back(distance_plan_all_vector[j]);
             }

             ROS_ERROR("11.计算圆弧路径  圆弧路径 arc_point.size() %i",arc_point.size());
             //圆弧路径
             for (int l = 0; l < arc_point.size(); ++l) {
                 distance_plan_temporary_vector.push_back(arc_point[l]);
             }
             ROS_ERROR("11.计算圆弧路径  下条路径 point_size_t %i",point_size_t);

             //下条路径
             for (int k = point_size_t; k <distance_plan_NEX.size(); ++k) {
                 distance_plan_temporary_vector.push_back(distance_plan_NEX[k]);
             }
                //todo 5.----------------计算圆弧路径-----------------------------------
             distance_plan_all_vector=distance_plan_temporary_vector;//赋值
             distance_plan_temporary_vector.clear();//清空临时容器
             //第二条路径是下一个节点的第一条路径
              onePlanToMap=onePlanToMap_NEX;

             ROS_ERROR("12.计算圆弧路径  所有路径 distance_plan_all_vector.size() %i",distance_plan_all_vector.size());


         }//end for (int i = 0; i < (onePlanToMaps_vector_.size()-1); ++i) {

         //遍历所有路径
         for (int i = 0; i < distance_plan_all_vector.size() ; ++i) {
             double world_x, world_y;
             //todo 像素坐标转世界坐标

             mapToWorld(distance_plan_all_vector[i].x, distance_plan_all_vector[i].y, world_x, world_y);
             geometry_msgs::PoseStamped pose;
             pose.pose.orientation.x = 0.0;
             pose.pose.orientation.y = 0.0;
             pose.pose.orientation.z = 0.0;
             pose.pose.orientation.w = 1.0;
             //设置 geometry_msgs::PoseStamped数据保存到plan路径中【plan 已经是世界坐标了，上一行代码  mapToWorld(result[0], result[1], world_x, world_y); 像素坐标转世界坐标】
             setPoseStampedPoseToPlan(pose,world_x,world_y, plan);
         }

         if (plan.size() > 0) {
             ROS_INFO("Found global plan");
             // add orientations if needed 如果需要，添加方向
             orientation_filter_->processPath(startPoseStamped, plan);
             ROS_INFO(" 圆弧 路径成功");
             return true;
         } else {
             ROS_ERROR("Failed to find global plan");
             return false;
         }

    }



    // 获取两条直线的交点（把交点作为控制点） line1_p1:直线1的起始点 line1_p2:直线1的结束点 line2_p1:直线2的起始点 line2_p2:直线2的结束点 intersection:交点（像素坐标） pc1:控制点（世界坐标） 返回值：true:有交点 false:无交点 ；[输入点坐标为像素坐标]
    bool global_planner::getIntersection(Point &line1_p1,Point &line1_p2,Point &line2_p1,Point &line2_p2,Point &intersection,geometry_msgs::PoseStamped &pc1){
        Line line1(line1_p1, line1_p2);
        Line line2(line2_p1, line2_p2);
        intersection = line1.getIntersection(line2);//获取交点 交点坐标为像素坐标 （把交点作为控制点）
        if (intersection.x == 0 && intersection.y == 0) {
            cout << "Lines are parallel or coincident. 直线平行或重合" << endl;
            return false;
        } else {
            cout << "Intersection point: (" << intersection.x << ", " << intersection.y << ")" << endl;
        }

        double world_x, world_y;
        //像素坐标转世界坐标
        mapToWorld((double)intersection.x , (double)intersection.y, world_x, world_y);
        pc1.header.stamp = ros::Time::now();
        pc1.header.frame_id = frame_id_;
        pc1.pose.position.x = world_x;
        pc1.pose.position.y = world_y;
        pc1.pose.position.z = 0.0;
        pc1.pose.orientation.x = 0.0;
        pc1.pose.orientation.y = 0.0;
        pc1.pose.orientation.z = 0.0;
        pc1.pose.orientation.w = 1.0;
        return true;
    }

    //---------------------------------------贝塞尔------------------------------------------------------- https://baike.baidu.com/item/%E8%B4%9D%E5%A1%9E%E5%B0%94%E6%9B%B2%E7%BA%BF/1091769?fr=aladdin
//设置 geometry_msgs::PoseStamped数据保存到plan路径中
    void global_planner::setPoseStampedPoseToPlan(geometry_msgs::PoseStamped &pose,double &x,double &y, std::vector<geometry_msgs::PoseStamped>& plan){
        pose.header.stamp = ros::Time::now();
        pose.header.frame_id = frame_id_;
        pose.pose.position.x = x;
        pose.pose.position.y = y;
        pose.pose.position.z = 0.0;
//        pose.pose.orientation.x = 0.0;
//        pose.pose.orientation.y = 0.0;
//        pose.pose.orientation.z = 0.0;
//        pose.pose.orientation.w = 1.0;
        plan.push_back(pose);
    }

    //设置 Point数据保存geometry_msgs::PoseStamped中
    void global_planner::setPoseStampedPoseWithPoint(Point point,geometry_msgs::PoseStamped &pose){
        pose.header.stamp = ros::Time::now();
        pose.header.frame_id = frame_id_;
        pose.pose.position.x = point.x;
        pose.pose.position.y = point.y;
        pose.pose.position.z = 0.0;
        pose.pose.orientation.x = 0.0;
        pose.pose.orientation.y = 0.0;
        pose.pose.orientation.z = 0.0;
        pose.pose.orientation.w = 1.0;

    }


    //一阶 Function to return point on Linear Bezier Curve
    //ps:起始点   pe:结束点  t: 0-1之间的数据；t:   绘制40个点，i从0开始计数到最大值40，计算  t = i / 40.0
    vector<double> global_planner::FirstBezier(vector<double> ps, vector<double> pe, double t)
   // vector<double> global_planner::FirstBezier(vector<double> pe, vector<double> ps, double t)
    {
        vector<double> point;
        point.push_back((1-t)*ps[0]+t*pe[0]);
        point.push_back((1-t)*ps[1]+t*pe[1]);
//        cout << "FirstBezier ps[0]:pe[0] " << to_string(ps[0]) << ", " << to_string(pe[0])  << endl;
//        cout << "FirstBezier ps[1]:pe[1] " << to_string(ps[1]) << ", " << to_string(pe[1])  << endl;
//        cout << "FirstBezier t "  << to_string(t)  << endl;
//        cout << "FirstBezier point[0]:point[1] " << to_string(point[0]) << ", " << to_string(point[1])  << endl;

        return point;
    }
    //二阶 Function to return point on Quadratic Bezier Curve
    //ps:起始点 pc:控制点  pe:结束点  t: 0-1之间的数据；t:   绘制40个点，i从0开始计数到最大值40，计算  t = i / 40.0
    vector<double> global_planner::QuadraticBezier(vector<double> ps, vector<double> pc, vector<double> pe, double t)
   // vector<double> global_planner::QuadraticBezier(vector<double> pe, vector<double> pc, vector<double> ps, double t)
    {
        vector<double> point;
        point.push_back(pow(1 - t, 2) * ps[0] + 2 * t * (1 - t) * pc[0] + pow(t, 2) * pe[0]);
        point.push_back(pow(1 - t, 2) * ps[1] + 2 * t * (1 - t) * pc[1] + pow(t, 2) * pe[1]);
        return point;
    }

    //三阶 Function to return point on Cubic Bezier Curve
    //ps:起始点 pc1:控制点1  pc2:控制点2 pe:结束点  t: 0-1之间的数据；t:   绘制40个点，i从0开始计数到最大值40，计算  t = i / 40.0
    vector<double> global_planner::CubicBezier(vector<double> ps, vector<double> pc1,vector<double> pc2, vector<double> pe, double t)
   // vector<double> global_planner::CubicBezier(vector<double> pe, vector<double> pc2,vector<double> pc1, vector<double> ps, double t)
    {
        vector<double> point;
        point.push_back(pow(1 - t, 3) * ps[0] + 3 * t * pow(1 - t, 2) * pc1[0] + 3 * pow(t, 2) * (1 - t) * pc2[0] + pow(t, 3) * pe[0]);
        point.push_back(pow(1 - t, 3) * ps[1] + 3 * t * pow(1 - t, 2) * pc1[1] + 3 * pow(t, 2) * (1 - t) * pc2[1] + pow(t, 3) * pe[1]);
        return point;
    }

    //四阶 Function to return point on Equation Bezier Curve
    //ps:起始点 pc1:控制点1  pc2:控制点2 pc3:控制点3 pe:结束点  t: 0-1之间的数据；t:   绘制40个点，i从0开始计数到最大值40，计算  t = i / 40.0
    vector<double> global_planner::EquationBezier(vector<double> ps, vector<double> pc1,vector<double> pc2, vector<double> pc3, vector<double> pe, double t)
   // vector<double> global_planner::EquationBezier(vector<double> pe, vector<double> pc3,vector<double> pc2, vector<double> pc1, vector<double> ps, double t)
    {
        vector<double> point;
        point.push_back(pow(1 - t, 4) * ps[0] + 4 * t * pow(1 - t, 3) * pc1[0] + 6 * pow(t, 2) * pow(1 - t, 2) * pc2[0] + 4 * pow(t, 3) * (1 - t) * pc3[0] + pow(t, 4) * pe[0]);
        point.push_back(pow(1 - t, 4) * ps[1] + 4 * t * pow(1 - t, 3) * pc1[1] + 6 * pow(t, 2) * pow(1 - t, 2) * pc2[1] + 4 * pow(t, 3) * (1 - t) * pc3[1] + pow(t, 4) * pe[1]);
        return point;
    }



    void global_planner::initialize() {

        ROS_WARN("1.my_global_planner_plugin 我的全局规划路径 initialize ");
        std::cout << "1.my_global_planner_plugin 我的全局规划路径 initialize " << std::endl;
        if(!initialized_){
            ROS_WARN("2.my_global_planner_plugin 我的全局规划路径 initialize ");
            std::cout << "2.my_global_planner_plugin 我的全局规划路径 initialize " << std::endl;
           // frame_id_ = planner_costmap_ros_->getGlobalFrameID();


            // initialize other planner parameters
            //ros::NodeHandle private_nh("~/" + global_planner_name_);
            //ros::NodeHandle private_nh2("~");
           // costmap_ = planner_costmap_ros_->getCostmap();//获取全局代价地图  //planner_costmap_ros_->getCostmap()
           // unsigned int cx = costmap_->getSizeInCellsX(), cy = costmap_->getSizeInCellsY();


            std::cout << "3.初始化 initialize costmap_ cx： " <<to_string(cx_)<<" , cy： " <<to_string(cy_)<<std::endl;

            p_calc_ = new PotentialCalculator(cx_, cy_);
            // DijkstraExpansion* de = new DijkstraExpansion(p_calc_, cx, cy);//true
            //de->setPreciseStart(true);
            //planner_ = de;
            //planner_ = new AStarExpansion(p_calc_, cx, cy);//false  // cx, cy 地图的大小，即像素数。



            // world_model_ = new base_local_planner::CostmapModel(*costmap_);
            ////方向滤波
            orientation_filter_ = new OrientationFilter();//进行方向滤波
            orientation_filter_->setMode(0);
            orientation_filter_->setWindowSize(0);
            initialized_ = true;
            grid_updated_ready_=true;//更新一个全局大地图
            ROS_WARN("3.my_global_planner_plugin 我的全局规划路径 initialize ");
            std::cout << "3.my_global_planner_plugin 我的全局规划路径 initialize " << std::endl;
        }
        else
            ROS_WARN("已经初始化了 This planner has already been initialized... doing nothing  我的全局规划路径 initialize");


    }

    void global_planner::run() {
        ros::NodeHandle private_nh("~");
        ros::NodeHandle private_nh2("~/" + global_planner_name_);
        //ros::NodeHandle action_nh("move_base_flex/move_base");//因为 move_base_flex中没有 move_base_simple/goal toptic，所以在这里需要制作一个与movebase中一致可用的 move_base_simple/goal
        ros::NodeHandle simple_nh("move_base_simple");//所以在这里需要制作一个与movebase 中一致可用的 move_base_simple/goal
        tf2_ros::TransformListener tfListener(buffer2);

        private_nh.param<string>("logFileName", logFileName, logFileName);//日志名称
        private_nh.param<string>("fileOutPut", fileOutPut_, fileOutPut_);//可配置路径
        private_nh.param<string>("move_base_global_toptic", move_base_global_toptic_, move_base_global_toptic_);//订阅全局地图   /move_base_flex/global_costmap/costmap  /move_base/global_costmap/costmap
        private_nh.param<string>("move_base_global_updates_toptic", move_base_global_updates_toptic_, move_base_global_updates_toptic_);// /move_base_flex/global_costmap/costmap_updates  /move_base/global_costmap/costmap_updates

        private_nh.param<string>("manyPath_toptic", manyPath_toptic_, manyPath_toptic_);//多路径发布  visualization_msgs::Marker
        private_nh.param<string>("manyPath_arc_toptic", manyPath_arc_toptic_, manyPath_arc_toptic_);//带圆弧的多路径发布  visualization_msgs::Marker
        private_nh.param<string>("manyPath_connect_toptic", manyPath_connect_toptic_, manyPath_connect_toptic_);//上一个路径与下一个路径的链接路径发布  visualization_msgs::Marker



        private_nh.param<string>("path_toptic", path_toptic_, path_toptic_);//one单路路径发布  nav_msgs::Path
        private_nh.param<string>("path_arc_toptic", path_arc_toptic_, path_arc_toptic_);//带圆弧的路径发布  nav_msgs::Path
        private_nh.param<string>("path_semiStructured_toptic", pathPlan_semiStructured_toptic_, pathPlan_semiStructured_toptic_);//半结构规划路径发布  nav_msgs::Path

        private_nh.param<double>("start_distance_threshold", start_distance_threshold_, start_distance_threshold_);//切入点最小距离阀值 单位m
        private_nh.param<double>("goal_distance_threshold", goal_distance_threshold_, goal_distance_threshold_);//切出点最小距离阀值 单位m

        private_nh.param<double>("startpose_distance_threshold", startpose_distance_threshold_, startpose_distance_threshold_);//切入点最小距离阀值， 判断起始点,判断终点是否在路径上 判断起始点,判断终点是否在路径上.小于这个阀值就认为在路径上
        private_nh.param<double>("goalpose_distance_threshold", goalpose_distance_threshold_, goalpose_distance_threshold_);//切出点最小距离阀值 。判断终点是否在路径上 判断起始点,判断终点是否在路径上，小于这个阀值就认为在路径上
        private_nh.param<string>("local_plan_toptic", local_plan_toptic_, local_plan_toptic_);//局部路径tooic名称

        private_nh.param<bool>("isthresholdClosestPoint", isthresholdClosestPoint_, isthresholdClosestPoint_);//true:选择最近点作为切入和切出点（切入点与切出点相对固定，取决于起点和终点的位置【满足 切入点最小距离阀值，切出点最小距离阀值的范围内选择】）， false：选择组合路径距离最短的点作为切入和切出点（满足  切入点最小距离阀值，切出点最小距离阀值的范围内选择）
        //是否保存当前计算出来的路径到  fileOutPut/planpose/plan.txt
        private_nh.param<bool>("isSavePlanToTxt", isSavePlanToTxt_, isSavePlanToTxt_);



        // private_nh.param<bool>("isUseSemiStructured", isUseSemiStructured_, isUseSemiStructured_);//是否使用半结构化规划路径


        private_nh.param<string>("clicked_point_toptic", clicked_point_toptic_, clicked_point_toptic_);//rviz中获取点击的点坐标

        private_nh.param("base_global_planner", global_planner_name_, std::string("contnav_global_planner/global_plannerROS"));//navfn/NavfnROS

        private_nh.param("robot_base_frame", robot_base_frame_, std::string("base_link"));
        //            //是否探索未知区域，flase--不可到达
        private_nh.param("allow_unknown", allow_unknown_, false);

        private_nh.param<int>("point_size_t", point_size_t_, point_size_t_);//绘制点数量，决定圆弧的大小 （多条路径点位数量 大于point_min_size_t_*2 或者只有2条直线时路径点位数量大于point_min_size_t_*1 绘制圆弧）
        private_nh.param<int>("point_min_size_t", point_min_size_t_, point_min_size_t_);//绘制点数量，决定圆弧的大小[对于短路径（路径点位数量小于point_min_size_t_ 为短路径），决定圆弧的大小] ，point_size_t_数据应当大于point_min_size_t_的数据
//            planner_->setHasUnknown(allow_unknown_);


        //窗口信息
        private_nh2.param("planner_window_x", planner_window_x_, 0.0);
        private_nh2.param("planner_window_y", planner_window_y_, 0.0);
        private_nh2.param("default_tolerance", default_tolerance_, 0.0);//#默认0.0，目标容差
        private_nh2.param("publish_scale", publish_scale_, 100);
        private_nh2.param("outline_map", outline_map_, true);

        /**
        这段代码是在创建一个名为"global_costmap"的costmap_2d::Costmap2DROS对象，
        其中tf_是一个tf::TransformListener对象。Costmap2DROS是一个ROS节点，
        它将costmap_2d::Costmap2D对象与ROS系统集成。它还提供了一些方便的功能，例如在ROS参数服务器上设置参数，以及在ROS中发布和订阅costmap。
        这个函数并没有直接订阅costmap数据。它创建了一个名为"global_costmap"的costmap_2d::Costmap2DROS对象，该对象将在ROS系统中发布和订阅costmap数据。在这个对象的构造函数中，它会调用costmap_2d::Costmap2DROS::start()函数，该函数会订阅ROS话题"/map"和"/map_updates"，并在ROS话题"/costmap_updates"上发布costmap数据。因此，这个函数创建的对象将订阅costmap数据。
        */
      //  controller_costmap_ros_ = new costmap_2d::Costmap2DROS("local_costmap", tf_);//局部规划地图
        //planner_costmap_ros_ = new costmap_2d::Costmap2DROS("global_costmap", tf_);//全局规划地图,//move_base的成本图, planner_costmap_ros_ = new costmap_2d::Costmap2DROS("global_costmap", tf_);//全局规划地图
        //planner_costmap_ros_->pause();//暂停全局代价地图的更新，但是不会关闭传感器话题的订阅
        //planner_costmap_ros_->start();//#订阅传感器话题，启动全局代价地图更新
       //planner_costmap_ros_->stop();//#停止全局代价地图更新，并停止订阅传感器话题
      //  planner_costmap_ros_->updateMap();//更新全局代价地图
      //  planner_costmap_ros_->resetLayers();//重置代价地图的所有图层
      //  planner_costmap_ros_->getCostmap()->setConvexPolygonCost(clear_poly, costmap_2d::FREE_SPACE);//设置多边形区域的代价值

        //cmd_vel_client_暂停速度话题
       // cmd_vel_client_ = nh_.serviceClient<std_srvs::SetBool>("/pause_cmd_vel");

        //---------------------------------------------------------------------------------------------------------------------------
        //feedbackCb中增加其它功能：1.检查机器人是否卡住。2.当机器人到达特定位置时执行其他操作（例如发布一条消息）。
        //custom_pub_ = nh_.advertise<std_msgs::String>("/custom_topic", 1);


        //todo 订阅一个全局大地图，做障碍和珊格地图的提取  move_base_flex/global_costmap/costmap
                                                                       //  /move_base/global_costmap/costmap    /move_base_flex/global_costmap/costmap
        costmap_sub_ = private_nh.subscribe<nav_msgs::OccupancyGrid>(move_base_global_toptic_, 1,
                                                             &global_planner::grid_callback, this);
        costmap_updated_sub_ = private_nh.subscribe<map_msgs::OccupancyGridUpdate>(
                move_base_global_updates_toptic_, 1,&global_planner::grid_updated_callback, this);

        //多路径发布   /contnav_global_planner_node/manyPath    https://blog.csdn.net/u013834525/article/details/80447931/?spm=1001.2014.3001.5506
        marker_pub_ = private_nh.advertise<visualization_msgs::Marker>(manyPath_toptic_, 10);


        //带圆弧的多路径发布   /contnav_global_planner_node/manyPath    https://blog.csdn.net/u013834525/article/details/80447931/?spm=1001.2014.3001.5506
        marker_arc_pub_ = private_nh.advertise<visualization_msgs::Marker>(manyPath_arc_toptic_, 10);

        //上一个路径与下一个路径的链接路径发布
        marker_connect_pub_ = private_nh.advertise<visualization_msgs::Marker>(manyPath_connect_toptic_, 1);

        //路径发布
        plan_pub_ = private_nh.advertise<nav_msgs::Path>(path_toptic_, 1);

        //带圆弧路径发布
        plan_arc_pub_ = private_nh.advertise<nav_msgs::Path>(path_arc_toptic_, 1);


        //todo 半结构规划路径发布   semiStructuredPathPlanningMakePlan          /planSemiStructured
        pathPlan_semiStructured_pub_ = private_nh.advertise<nav_msgs::Path>(pathPlan_semiStructured_toptic_, 1);

        //发布代价地图
        potential_pub_ = private_nh.advertise<nav_msgs::OccupancyGrid>("potential", 1);



      //---------------------------------------------------------------------------------------------------------------


        //恢复代价地图
          clear_costmaps_client_ = nh_.serviceClient<std_srvs::Empty>("move_base_flex/clear_costmaps");

        //日志写入
        contnavLoggerClient = nh_.serviceClient<contnav_srvs::ContnavLogger::Request, contnav_srvs::ContnavLogger::Response>(
                "/contnavLogger");
        //数据库操作
//        SqlOperationClient = nh_.serviceClient<contnav_srvs::TABLE_CONTORLLER::Request, contnav_srvs::TABLE_CONTORLLER::Response>(
//                "/SqlOperation");


        subscriber_refreshdb = nh_.subscribe<std_msgs::String>("/refreshdb", 10, &global_planner::callBack_refreshdb, this);
        //接收控制指令
        global_plannerServer = nh_.advertiseService<contnav_srvs::global_planner::Request, contnav_srvs::global_planner::Response>(
                "/my_global_planner", boost::bind(&global_planner::callback_global_plannerFun, this, _1, _2));


        //rviz中获取点击的点坐标
          waypoint_sub_ = nh_.subscribe(clicked_point_toptic_, 100, &global_planner::waypointCallback, this);

          //发布速度
      //  cmd_vel_pub_ = nh_.advertise<geometry_msgs::Twist>("/cmd_vel", 10);

        //定时任务，这里会阻塞，因为这里需要查询数据库，需要提前启动数据库节点
       // timer2_ = nh_.createTimer(ros::Duration(1), &global_planner::updatefun, this, false, true);//数据表数据更新后2秒刷新数据
         //执行状态机
        boost::thread parse_thread(boost::bind(&global_planner::thread_msg, this));
         //发布速度停止指令
      //  boost::thread stopcmd_thread(boost::bind(&global_planner::stopCmd_thread, this));
//
//
        dsrv_ = new dynamic_reconfigure::Server<contnav_global_planner::ContnavGlobalPlannerConfig>(ros::NodeHandle("~"));
        dynamic_reconfigure::Server<contnav_global_planner::ContnavGlobalPlannerConfig>::CallbackType cb = boost::bind(&global_planner::reconfigureCB, this, _1, _2);
        dsrv_->setCallback(cb);

        //private_nh.param<std::string>("map_frame", map_frame_, "map");
        //sub_ = node.subscribe<contnav_msgs::***>("/registerServiceResult", 10, &global_planner::msg_callBack, this);
        ros::spin();
    }







    //获取数据 boost::any 中的数据，函数的输入参数包括 boost::any 对象和其期望的类型，函数的返回值是类型转换后的值
    template<typename T> T global_planner::any_cast(const boost::any & operand)
    {
        try
        {
            return boost::any_cast<T>(operand);
        }
        catch (boost::bad_any_cast &)
        {
            ROS_ERROR("获取数据 boost::any 中的数据类型转换失败 ,Failed to cast %s to %s", operand.type().name(), typeid(T).name());
            throw;
        }
    }






// 调用 MBF 客户端导航至目标点
        void global_planner::navigate_to_goalpath(const std::vector<boost::any> &args) {
        ROS_INFO("Navigating to navigate_to_goalpath...");
        ROS_INFO_STREAM("Type of args[1]: " << args[1].type().name());
        ROS_INFO_STREAM("Type of args[3]: " << args[3].type().name());

        geometry_msgs::PoseStamped start_pose, target_pose, target_pose_default;
        //正在执行
        std::vector<geometry_msgs::PoseStamped> plan_bendi;
        std::string extra_string="EMPTY";
        try
        {

            boost::any age0=args[0];
            boost::any age1=args[1];
            boost::any age2=args[2];
            boost::any age3=args[3];
            boost::any age4=args[4];
            state_machine_.get_current_state_param(0,age0);
            state_machine_.get_current_state_param(1,age1);
            state_machine_.get_current_state_param(2,age2);
            state_machine_.get_current_state_param(3,age3);
            state_machine_.get_current_state_param(4,age4);
            start_pose = any_cast<geometry_msgs::PoseStamped>(age0);
            target_pose = any_cast<geometry_msgs::PoseStamped>(age1);
            target_pose_default = any_cast<geometry_msgs::PoseStamped>(age2);
            extra_string = any_cast<std::string>(age3);
            plan_bendi = any_cast<std::vector<geometry_msgs::PoseStamped>>(age4);
        }
        catch(...)
        {
            // handle the exception
            ROS_ERROR("1. navigate_to_goalpath 获取状态机数据 失败extra_string %s", extra_string.c_str() );
            return ;
        }


        if(state_machine_.get_current_state()=="navigate_to_goalpath" && extra_string=="EMPTY"){
            //切换正在执行状态
            int state_params_size=state_machine_.set_state("executing", {start_pose,target_pose,target_pose_default, std::string("navigate_to_goalpath"),plan_bendi});

        }else if(state_machine_.get_current_state()=="navigate_to_goalpath" && extra_string=="navigate_to_goalpathAain") {
            //切换正在执行状态
            int state_params_size=state_machine_.set_state("executing", {start_pose,target_pose,target_pose_default, std::string("navigate_to_goalpathAain"),plan_bendi});

        }


        ROS_INFO_STREAM("Type of extra_string: " << extra_string);
       // geometry_msgs::PoseStamped target_pose = boost::any_cast<geometry_msgs::PoseStamped>(args);
        target_pose_ = target_pose;//目标点位置
        //获取小车当前位置
        geometry_msgs::PoseStamped robot_pose;
        if(!getRobotPose(robot_pose)){
            ROS_ERROR("my_global_planner cannot make a plan for you because it could not get the start pose of the robot");
            //日志
            //  contnav_error(__FILE__,__LINE__,const_cast<char*>(response.message.c_str()));
            //  contnav_error(__FILE__,__LINE__,"global_planner::goalCB 获取小车当前位置 error");
            return ;
        }
       // mbf_msgs::ExePathGoal result_goal_path;
        std::vector<geometry_msgs::PoseStamped> plan;
        bool sendGoalPlanfig= sendGoalPlan(robot_pose,target_pose,plan);
        if(sendGoalPlanfig){
            //设置next_state下一个状态和参数  navigate_to_goalPath
            if(state_machine_.get_current_state()=="executing" )
            int state_params_size=state_machine_.set_state("monitor_progress", {start_pose,target_pose,target_pose_default, std::string("success"),plan});
        }else{
            ROS_DEBUG_NAMED("goalCB ","半结构化路径规划失败");
            std::cout << " 清空代价地图 "  << std::endl;
            //恢复代价地图
            std_srvs::Empty srv;
            clear_costmaps_client_.call(srv);
            if(state_machine_.get_current_state()=="executing" ){
                int state_params_size=state_machine_.set_state("monitor_progress", {start_pose,target_pose,target_pose_default, std::string("fail"),plan});
                ROS_ERROR("navigate_to_goalpath  : %s", state_machine_.get_current_state().c_str() );
            }

        }

    }


    // 调用 MBF 客户端导航至目标点 (到默认路径点)
    void global_planner::navigate_to_goalpathOverturn(const std::vector<boost::any> &args) {

        ROS_INFO("Navigating to navigate_to_goalpathOverturn ...");
        ROS_INFO_STREAM("Type of args[1]: " << args[1].type().name());
//        geometry_msgs::PoseStamped start_pose = boost::any_cast<geometry_msgs::PoseStamped>(args[0]);
//        geometry_msgs::PoseStamped target_pose = boost::any_cast<geometry_msgs::PoseStamped>(args[1]);
//        geometry_msgs::PoseStamped target_pose_default = boost::any_cast<geometry_msgs::PoseStamped>(args[2]);//默认点位，可以从数据库中读取，可以回充电装
//        std::string extra_string = boost::any_cast<std::string>(args[3]);


        //获取数据
        geometry_msgs::PoseStamped start_pose, target_pose, target_pose_default;
        //正在执行
        std::vector<geometry_msgs::PoseStamped> plan_bendi;
        std::string extra_string="EMPTY";
        try
        {
            boost::any age0=args[0];
            boost::any age1=args[1];
            boost::any age2=args[2];
            boost::any age3=args[3];
            boost::any age4=args[4];
            state_machine_.get_current_state_param(0,age0);
            state_machine_.get_current_state_param(1,age1);
            state_machine_.get_current_state_param(2,age2);
            state_machine_.get_current_state_param(3,age3);
            state_machine_.get_current_state_param(4,age4);
            start_pose = any_cast<geometry_msgs::PoseStamped>(age0);
            target_pose = any_cast<geometry_msgs::PoseStamped>(age1);
            target_pose_default = any_cast<geometry_msgs::PoseStamped>(age2);
            extra_string = any_cast<std::string>(age3);
            plan_bendi = any_cast<std::vector<geometry_msgs::PoseStamped>>(age4);
        }
        catch(...)
        {
            // handle the exception
            ROS_ERROR("1. navigate_to_goalpath 获取状态机数据 失败extra_string %s", extra_string.c_str() );
            return ;
        }

        if(state_machine_.get_current_state()=="navigate_to_goalpathOverturn" && extra_string=="EMPTY"){
            //切换正在执行状态
            int state_params_size=state_machine_.set_state("executing", {start_pose,target_pose,target_pose_default, std::string("navigate_to_goalpathOverturn"),plan_bendi});

        }else if(state_machine_.get_current_state()=="navigate_to_goalpathOverturn" && extra_string=="navigate_to_goalpathOverturnAain") {
            //切换正在执行状态
            int state_params_size=state_machine_.set_state("executing", {start_pose,target_pose,target_pose_default, std::string("navigate_to_goalpathOverturnAain"),plan_bendi});

        }



        //

        // geometry_msgs::PoseStamped target_pose = boost::any_cast<geometry_msgs::PoseStamped>(args);
        target_pose_ = target_pose;//目标点位置
        //获取小车当前位置
        geometry_msgs::PoseStamped robot_pose;
        if(!getRobotPose(robot_pose)){
            ROS_ERROR("my_global_planner cannot make a plan for you because it could not get the start pose of the robot");
            //日志
            //  contnav_error(__FILE__,__LINE__,const_cast<char*>(response.message.c_str()));
            //  contnav_error(__FILE__,__LINE__,"global_planner::goalCB 获取小车当前位置 error");
            return ;
        }
        // mbf_msgs::ExePathGoal result_goal_path;
        std::vector<geometry_msgs::PoseStamped> plan;
        // bool sendGoalPlanfig= sendGoalPlanOverturn(robot_pose,start_pose,result_goal_path);//翻转，走向起点
        bool sendGoalPlanfig= sendGoalPlanOverturn(robot_pose,target_pose,plan);//翻转，走向起点，可以回充电装
        if(sendGoalPlanfig){

            //设置next_state下一个状态和参数
            int state_params_size= state_machine_.set_state("monitor_progress", {start_pose,target_pose,target_pose_default, std::string("success"),plan});


        }else{
            ROS_DEBUG_NAMED("goalCB ","半结构化路径规划失败");
            std::cout << " 清空代价地图 "  << std::endl;
            //恢复代价地图
            std_srvs::Empty srv;
            clear_costmaps_client_.call(srv);

            int state_params_size=state_machine_.set_state("monitor_progress", {robot_pose,target_pose,target_pose_default, std::string("fail") ,plan});
            cout<<"======int state_params_size======navigate_to_goalpath=fail==monitor_progress===state_params_size: "<<state_params_size<<endl;



        }

    }



    bool   global_planner::sendGoalPlan(geometry_msgs::PoseStamped &robot_pose, geometry_msgs::PoseStamped &goal,std::vector<geometry_msgs::PoseStamped> &plan){


     geometry_msgs::PoseStamped start_pose, goal_pose;
     //起始位置
     start_pose.header.stamp = ros::Time::now();
     start_pose.header.frame_id=frame_id_;
     start_pose.pose = robot_pose.pose;
     //目标位置
     goal_pose = goal;



     std::cout << "6.路径规划 ================================================== " << std::endl;
    // std::vector<geometry_msgs::PoseStamped> plan;
     //todo 3.规划路径
     if(makePlan2(start_pose,  goal_pose,default_tolerance_,plan)){
         std::cout << "sussed 路径规划 makePlan my_global_planner_plugin 我的全局规划路径  " << std::endl;
         string message=" 路径规划 makePlan 已经规划出路径来！ ";
         //日志
         //  contnav_info(__FILE__,__LINE__,const_cast<char*>(response.message.c_str()));
         //  contnav_info(__FILE__,__LINE__,"text");
     }else{
         std::cout << "error 路径规划 makePlan2 my_global_planner_plugin 我的全局规划路径  " << std::endl;
         string message="error 路径规划 makePlan 未规划出路径来！ ";
         //日志
         //  contnav_error(__FILE__,__LINE__,const_cast<char*>(response.message.c_str()));
         //  contnav_error(__FILE__,__LINE__,"text");
         return false;
     }

     // 在第一个位置插入数据 起始数据
     // plan.insert(plan.begin(), start_pose);
     // std::vector集合中的第一个位置覆盖第一个数据


     std::cout << "1. 路径优化 makePlan2 my_global_planner_plugin plan.size() " <<plan.size()<< std::endl;
     // Optimize the global path
//        double distance_threshold = 0.1; // Adjust this value according to your needs 选择较小的阈值，以保留更多的路径点
//        int window_size = 5; // Adjust this value 您可以选择较小的窗口大小，以保留路径的形状
//        // Adjust this value according to your needs 路径优化
//        optimize_global_path(plan, distance_threshold, window_size);


     // Path optimization
     double max_distance = 0.5;
     int window_size = 2;
     insert_intermediate_points(plan, max_distance);
     smooth_path(plan, window_size);
     calculate_orientations(plan);

// Print the optimized path
//     for (const auto& pose : plan) {
//         std::cout << "x: " << pose.pose.position.x
//                   << ", y: " << pose.pose.position.y
//                   << ", z: " << pose.pose.position.z
//                   << ", qx: " << pose.pose.orientation.x
//                   << ", qy: " << pose.pose.orientation.y
//                   << ", qz: " << pose.pose.orientation.z
//                   << ", qw: " << pose.pose.orientation.w
//                   << std::endl;
//     }


     if(plan.size()>0){//添加启示点与结束点
         plan[0]=start_pose;
         plan[plan.size()-1]=goal_pose;
     }

     std::cout << "2. 路径优化 makePlan2 my_global_planner_plugin plan.size() " <<plan.size()<< std::endl;

     if(isSavePlanToTxt_){
         //保存路径到文件txt中以便查看debug
         std::string filename = "plan.txt";
         std::string directory = fileOutPut_+"/planpose";
         createDirectory(directory);
         std::string filepath = directory + "/" + filename;
         writeEulerAnglesToFile(plan, filepath);
     }


     //发送路径导航
    // mbf_msgs::ExePathGoal result_goal_path;
//     for (int i = 0; i < plan.size(); ++i) {
//         plan[i].header.frame_id=frame_id_;
//         plan[i].header.stamp=ros::Time::now();
//         result_goal_path.path.poses.push_back(plan[i]);
//     }
     //goal_msg.path.poses.push_back(goal_pose);
     //记录全局路径
   //  global_path_=result_goal_path;

     // 发送导航目标到 /move_base_flex/exe_path 话题
   //  ac_.sendGoal(result_goal_path, boost::bind(&global_planner::resultCallback, this, _1, _2));
     // 等待路径规划请求完成
     // 等待路径规划请求完成
//     bool finished_before_timeout = ac_.waitForResult(ros::Duration(30.0));
//
//     // 如果请求完成前已经超时，则取消该请求
//     if (!finished_before_timeout)
//     {
//         ROS_WARN("1如果请求完成前已经超时，则取消该请求Path planning goal timed out. Cancelling the goal.");
//         ac_.cancelGoal();
//         ROS_INFO("2如果请求完成前已经超时，则取消该请求 Path planning goal cancelled.");
//     }else{
//         ROS_DEBUG_NAMED("goalCB ","2.第二次半结构化路径规划成功");
//     }
     return true;
     //end----------------------------------------------半结构化路径规划----------------------------------------------
    }

    //翻转路径
    bool   global_planner::sendGoalPlanOverturn(geometry_msgs::PoseStamped &robot_pose, geometry_msgs::PoseStamped &goal,std::vector<geometry_msgs::PoseStamped> &plan){


        geometry_msgs::PoseStamped start_pose, goal_pose;
        //起始位置
        start_pose.header.stamp = ros::Time::now();
        start_pose.header.frame_id=frame_id_;
        start_pose.pose = robot_pose.pose;
        //目标位置
        goal_pose = goal;



        std::cout << "6.路径规划 ===================makePlan2Overturn=============================== " << std::endl;
      //  std::vector<geometry_msgs::PoseStamped> plan;
        //todo 3.规划路径
        if(makePlan2Overturn(start_pose,  goal_pose,default_tolerance_,plan)){
            std::cout << "sussed 路径规划 makePlan2Overturn my_global_planner_plugin 我的全局规划路径  " << std::endl;
            string message=" 路径规划 makePlan2Overturn 已经规划出路径来！ ";
            //日志
            //  contnav_info(__FILE__,__LINE__,const_cast<char*>(response.message.c_str()));
            //  contnav_info(__FILE__,__LINE__,"text");

        }else{
            std::cout << "error 路径规划 makePlan2Overturn my_global_planner_plugin 我的全局规划路径  " << std::endl;
            string message="error 路径规划 makePlan2Overturn 未规划出路径来！ ";
            //日志
            //  contnav_error(__FILE__,__LINE__,const_cast<char*>(response.message.c_str()));
            //  contnav_error(__FILE__,__LINE__,"text");
            return false;
        }

        // 在第一个位置插入数据 起始数据
        // plan.insert(plan.begin(), start_pose);
        // std::vector集合中的第一个位置覆盖第一个数据


        std::cout << "1. 路径优化 makePlan2Overturn my_global_planner_plugin plan.size() " <<plan.size()<< std::endl;
        // Optimize the global path
//        double distance_threshold = 0.1; // Adjust this value according to your needs 选择较小的阈值，以保留更多的路径点
//        int window_size = 5; // Adjust this value 您可以选择较小的窗口大小，以保留路径的形状
//        // Adjust this value according to your needs 路径优化
//        optimize_global_path(plan, distance_threshold, window_size);


        // Path optimization
        double max_distance = 0.5;
        int window_size = 2;
        insert_intermediate_points(plan, max_distance);
        smooth_path(plan, window_size);
        calculate_orientations(plan);

// Print the optimized path
//     for (const auto& pose : plan) {
//         std::cout << "x: " << pose.pose.position.x
//                   << ", y: " << pose.pose.position.y
//                   << ", z: " << pose.pose.position.z
//                   << ", qx: " << pose.pose.orientation.x
//                   << ", qy: " << pose.pose.orientation.y
//                   << ", qz: " << pose.pose.orientation.z
//                   << ", qw: " << pose.pose.orientation.w
//                   << std::endl;
//     }


        if(plan.size()>0){//添加启示点与结束点
            plan[0]=start_pose;
            plan[plan.size()-1]=goal_pose;
        }

        std::cout << "2. 路径优化 makePlan2Overturn my_global_planner_plugin plan.size() " <<plan.size()<< std::endl;
        if(isSavePlanToTxt_){
            //保存路径到文件txt中以便查看debug
            std::string filename = "plan.txt";
            std::string directory = fileOutPut_+"/planpose";
            createDirectory(directory);//创建文件夹
            std::string filepath = directory + "/" + filename;
            writeEulerAnglesToFile(plan, filepath);//保存路径到文件txt中以便查看debug
        }


        //发送路径导航
        // mbf_msgs::ExePathGoal result_goal_path;
//        for (int i = 0; i < plan.size(); ++i) {
//            plan[i].header.frame_id=frame_id_;
//            plan[i].header.stamp=ros::Time::now();
//            result_goal_path.path.poses.push_back(plan[i]);
//        }
        //goal_msg.path.poses.push_back(goal_pose);
        //记录全局路径
        //global_path_=result_goal_path;

        // 发送导航目标到 /move_base_flex/exe_path 话题
        //  ac_.sendGoal(result_goal_path, boost::bind(&global_planner::resultCallback, this, _1, _2));
        // 等待路径规划请求完成
        // 等待路径规划请求完成
//     bool finished_before_timeout = ac_.waitForResult(ros::Duration(30.0));
//
//     // 如果请求完成前已经超时，则取消该请求
//     if (!finished_before_timeout)
//     {
//         ROS_WARN("1如果请求完成前已经超时，则取消该请求Path planning goal timed out. Cancelling the goal.");
//         ac_.cancelGoal();
//         ROS_INFO("2如果请求完成前已经超时，则取消该请求 Path planning goal cancelled.");
//     }else{
//         ROS_DEBUG_NAMED("goalCB ","2.第二次半结构化路径规划成功");
//     }
        return true;
        //end----------------------------------------------半结构化路径规划----------------------------------------------
    }






    void global_planner::init() {
       /// stopCmdFig_ = false;//发布速度停止指令
    //    std_srvs::SetBool srv1;
  //      srv1.request.data = false;  // 设置为 true 以启用 cmd_vel 发布，设置为 false 以禁用发布
//        if (cmd_vel_client_.call(srv1)) {
//            ROS_INFO("Service call succeeded 发布速度开始指令给movebase: %s", srv1.response.message.c_str());
//        } else {
//            ROS_ERROR("Failed to call service toggle_cmd_vel  发布速度开始指令给movebase ");
//        }
        //初始化给一个控制
        geometry_msgs::PoseStamped start_pose,target_pose,target_pose_default;
        state_machine_.set_state("idle", {start_pose,target_pose,target_pose_default,std::string("EMPTY")});//闲置状态,初始化给一个控制
        execute_map_plan_.clear();//清除容器中所有半结构化路径
        execute_map_plan_Overturn_.clear();//清除容器中所有半结构化路径
        plan_all_indexs_.clear();//清除所有半结构化路径路径在地图中的索引
        current_path_goTo_target_path_map_plan_.clear();//清除当前路径到达目标路径的路径数据
        current_path_goTo_target_path_map_plan_Overturn_.clear();//清除当前路径到达目标路径的路径数据
        data_ready = false;//恢复数据，用互斥锁保证数据的安全
      //  mbf_msgs::ExePathGoal global_path;//未走过的路径
      //  mbf_msgs::ExePathGoal  traversed_path;//添加到已走过的路径中
     //   global_path_=global_path;//清除全局路径
      //  traversed_path_=traversed_path;//清除已经走过的路径


     //   obstacle_detected_time_= ros::Time(0);;//对障碍物进行处理，这里记录时间，如果时间超过一定时间，就认为做相应的处理
        std::cout << " 清空代价地图 "  << std::endl;
        //恢复代价地图
        std_srvs::Empty srv;
        clear_costmaps_client_.call(srv);

    }

//参数服务器
    void global_planner::reconfigureCB(contnav_global_planner::ContnavGlobalPlannerConfig &config, uint32_t level){
        boost::recursive_mutex::scoped_lock l(configuration_mutex_);
        std::cout << "启动参数服务器 " << std::endl;
        /**
         * todo 这里逻辑需要重写
            if(planner_!= nullptr){//要先创建planner_对象，目前是在service回调函数中创建的所以这里不能去除判断，否则会报错；这里逻辑需要重写
                planner_->setLethalCost(config.lethal_cost);
                planner_->setNeutralCost(config.neutral_cost);
                planner_->setFactor(config.cost_factor);
            }
        */
    }

   //rviz 成功接收到了点的坐标信息
    void global_planner::waypointCallback(const geometry_msgs::PointStampedConstPtr& waypoint) {

        //成功接收到了点的坐标信息
        ROS_INFO("pose show start rviz 成功接收到了点的坐标信息 !");
        ROS_INFO("pose is %f", waypoint->header.stamp.toSec());
        ROS_INFO("pose is %f", waypoint->point.x);
        ROS_INFO("pose is %f", waypoint->point.y);
        ROS_INFO("pose is %f", waypoint->point.z);
       geometry_msgs::PointStamped point = *waypoint;
       //geometry_msgs::PointStamped与geometry_msgs::PoseStamped互转
       geometry_msgs::PoseStamped waypointStamped = pointStampedToPose(point);
       waypoints_.push_back(waypointStamped);

    }


//geometry_msgs::PointStamped与geometry_msgs::PoseStamped互转
    geometry_msgs::PoseStamped global_planner::pointStampedToPose( geometry_msgs::PointStamped& pointStamped)
    {
        geometry_msgs::PoseStamped poseStamped;
        poseStamped.header = pointStamped.header;
        poseStamped.pose.position.x = pointStamped.point.x;
        poseStamped.pose.position.y = pointStamped.point.y;
        poseStamped.pose.position.z = pointStamped.point.z;
        poseStamped.pose.orientation.x = 0;
        poseStamped.pose.orientation.y = 0;
        poseStamped.pose.orientation.z = 0;
        poseStamped.pose.orientation.w = 1;

        return poseStamped;
    }



    //-------------------------------------------------------
    /**
            sukai@sukai:~/workspace/workspace_ros_car_noetic$ rostopic type /move_base/global_costmap/costmap
            nav_msgs/OccupancyGrid
            sukai@sukai:~/workspace/workspace_ros_car_noetic$ rostopic type /map
            nav_msgs/OccupancyGrid
            sukai@sukai:~/workspace/workspace_ros_car_noetic$
     */

    //订阅一个全局大地图，做障碍和珊格地图的提取
    void global_planner::grid_callback(nav_msgs::OccupancyGridConstPtr const& msg) {
        std::cout << "*******1.订阅一个全局大地图，做障碍和珊格地图的提取 frame_id_：****** " <<frame_id_<<std::endl;
        grid_ready_ = false;
        grid_ = *msg;
        frame_id_ = msg->header.frame_id;
        std::cout << "1.订阅一个全局大地图，做障碍和珊格地图的提取 frame_id_： " <<frame_id_<<std::endl;
        // 格栅地图转换为代价地图
        origin_costmap_ = grid2costmap(grid_);
        cx_ = origin_costmap_->getSizeInCellsX(), cy_ = origin_costmap_->getSizeInCellsY();
        setSize( cx_,  cy_);
        grid_ready_ = true;

    }

    void global_planner::grid_updated_callback(map_msgs::OccupancyGridUpdateConstPtr const& msg) {
        if (!grid_ready_) return;
       // std::cout << "*******2.grid_updated_callback 更新一个全局大地图，做障碍和珊格地图的提取 frame_id_：****** " <<frame_id_<<std::endl;

        grid_updated_ready_ = false;
        int index = 0;
        for (unsigned y = msg->y; y < msg->y + msg->height; ++y) {
            for (unsigned x = msg->x; x < msg->x + msg->width; ++x) {
                grid_.data[y * grid_.info.width + x] = msg->data[index++];
            }
        }
        // 格栅地图转换为代价地图
        origin_costmap_ = grid2costmap(grid_);
        cx_ = origin_costmap_->getSizeInCellsX(), cy_ = origin_costmap_->getSizeInCellsY();
        setSize( cx_,  cy_);
        grid_updated_ready_ = true;
    }


    /**
可选参数：

mode: Trinary是默认值三元解释，即解释所有值，使输出最终成为三个值之一

灰度值 [0,255] 转换成p值 [0,1]

转换公式 p=(255-x)/255.0

---

        4.map与costmap都是以nav_msgs::OccupancyGrid类型发布其topic。其中整张地图的障碍物信息存放在data数据成员中，data是一个int8类型的vector向量，即一维数组。假设一张pgm的map地图，宽：width，高：height，单位为像素，分辨路为resolution，左下角像素点在世界坐标系下的位置为：（ox,oy）,单位米，那么世界坐标系下一点（x,y）单位米，假设其在地图中，那么该点对应的data中的索引index为：
        index = (x-ox)/resolution+(y-oy)/resolution*width
        （注：index为int8类型，所以要将结果进行下类型转换），

        那么该点在地图中的信息即为data[index]
        即：data是按照那张地图图片的自底向上，自左至右逐个像素点存储的
        map中data存储的格式如下：
        0:空白区域
        100:障碍物
        -1:未知
        1-99:根据像素的灰度值转换后的值，代表有障碍物的概率（不确定，猜测是这个含义，实际用的时候把这个值都当作无障碍物）
        costmap中data存储格式如下：
        0:空白区域
        100:障碍物层
        99:膨胀层
        1-98:距离障碍物层的远近获得的代价值
        -1:未知区域


        打印nav_msgs::OccupancyGrid发现，data中只存有0,100，-1，三种信息，没有1-99
        ————————————————
        版权声明：本文为CSDN博主「再遇当年」的原创文章，遵循CC 4.0 BY-SA版权协议，转载请附上原文出处链接及本声明。
        原文链接：https://blog.csdn.net/qq_15204179/article/details/121395464
     */


//格栅地图转换为代价地图    //https://blog.csdn.net/qq_35635374/article/details/120918847 二维占据栅格地图costmap生成方法及源码解析
    auto
    global_planner::grid2costmap(nav_msgs::OccupancyGrid const& grid) const -> std::shared_ptr<costmap_2d::Costmap2D> {
        auto costmap = std::make_shared<costmap_2d::Costmap2D>(grid.info.width,
                                                               grid.info.height,
                                                               grid.info.resolution,
                                                               grid.info.origin.position.x,
                                                               grid.info.origin.position.y);
        costmap->setDefaultValue(255);

        costmap->resetMap(0, 0, costmap->getSizeInCellsX(), costmap->getSizeInCellsY());
        for (unsigned y = 0; y < grid.info.height; y++) {
            for (unsigned x = 0; x < grid.info.width; x++) {
                if (grid.data[y * grid.info.width + x] < 0) {
                    /**
                    static const unsigned char NO_INFORMATION = 255;
                    static const unsigned char LETHAL_OBSTACLE = 254;
                    static const unsigned char INSCRIBED_INFLATED_OBSTACLE = 253;
                    static const unsigned char FREE_SPACE = 0;
                    */
                    costmap->setCost(x, y, costmap_2d::NO_INFORMATION);//255
                    continue;
                }



                //   100:障碍物层     99:膨胀层    1-98:距离障碍物层的远近获得的代价值
                if (grid.data[y * grid.info.width + x] >98&& grid.data[y * grid.info.width + x] <=100) {//苏凯 自己加的代码
                    /**
                    static const unsigned char NO_INFORMATION = 255;
                    static const unsigned char LETHAL_OBSTACLE = 254;
                    static const unsigned char INSCRIBED_INFLATED_OBSTACLE = 253;
                    static const unsigned char FREE_SPACE = 0;
                    */
                    costmap->setCost(x, y, costmap_2d::LETHAL_OBSTACLE);//254
                    continue;
                }

                /**
                1-99:根据像素的灰度值转换后的值，代表有障碍物的概率（不确定，猜测是这个含义，实际用的时候把这个值都当作无障碍物）
                costmap中data存储格式如下：
                0:空白区域
                100:障碍物层
                99:膨胀层
                1-98:距离障碍物层的远近获得的代价值
                -1:未知区域
                */
                costmap->setCost(x, y, static_cast<uint8_t>(grid.data[y * grid.info.width + x] * 254 / 100));
            }
        }

        return costmap;
    }
//代价地图转换为cv::Mat
/**
    auto global_planner::costmap2cv_mat(std::shared_ptr<costmap_2d::Costmap2D> const& costmap,
                                             int32_t x,
                                             int32_t y,
                                             int32_t width,
                                             int32_t height) -> cv::Mat {
        auto ret_mat = cv::Mat(height, width, CV_8U);
        for (auto r = 0; r < height; r++) {
            for (auto c = 0; c < width; c++) {
                //costmap's origin is at left bottom ,while opencv's pic's origin is at left-top.
                auto mx = c + x;
                auto my = y + height - r - 1;
                if (mx < 0 || mx >= costmap->getSizeInCellsX() || my < 0 || my >= costmap->getSizeInCellsY()) {
                    continue;
                }

                ret_mat.at<uchar>(r, c)
                        = costmap->getCost(static_cast<unsigned>(mx), static_cast<unsigned>(my));
            }
        }

        return ret_mat;
    }
*/
//----------------------------------------------------


    bool global_planner::getuwb_tf(string &pframe_id,string &cframe_id,geometry_msgs::TransformStamped &ps_out) {

        try{


            ps_out = buffer2.lookupTransform(pframe_id, cframe_id, ros::Time(0),ros::Duration(2));
//            ROS_INFO("相对关系信息:父级：%s,子级：%s 偏移量(%.2f,%.2f,%.2f) 4元素(qx %.5f,qy  %.5f,qz %.5f,qw  %.5f)",
//                     ps_out.header.frame_id.c_str(), // son2
//                     ps_out.child_frame_id.c_str(),  // son1
//                     ps_out.transform.translation.x,
//                     ps_out.transform.translation.y,
//                     ps_out.transform.translation.z,
//                     ps_out.transform.rotation.x,
//                     ps_out.transform.rotation.y,
//                     ps_out.transform.rotation.z,
//                     ps_out.transform.rotation.w
//            );
        }catch (exception e){
            std::cout << "=======getuwb_urdf=====异常===================" << std::endl;
            return false;
        }

        return true;
    }



//================================================
    //清除成本图中的起始单元格，因为我们知道它不会成为障碍
    void global_planner::clearRobotCell(const geometry_msgs::PoseStamped& global_pose, unsigned int mx, unsigned int my) {
        if (!initialized_) {
            ROS_ERROR(
                    "This planner has not been initialized yet, but it is being used, please call initialize() before use");
            return;
        }

        //set the associated costs in the cost map to be free
        origin_costmap_->setCost(mx, my, costmap_2d::FREE_SPACE);//自由空间
    }


    /**
 * 具体而言，该函数的实现方式是，分别用指针 pc 指向数组的第一行、最后一行、第一列和最后一列，
 * 然后使用循环依次遍历这四个边界，并将它们的每个元素设置为指定的 value 值。

在代码的实现过程中，nx 和 ny 分别表示数组 costarr 的宽度和高度，unsigned char 是一个无符号字符类型，用
 于存储 8 位无符号整数。因此，该函数的输入参数包括一个指向无符号字符数组的指针 costarr，数组的宽度 nx 和高度 ny，以及要设置的值 value。
 * @param costarr
 * @param nx
 * @param ny
 * @param value
 */
    void global_planner::outlineMap(unsigned char* costarr, int nx, int ny, unsigned char value) {
        // 首先，将数组第一行全部设置为指定的值
        unsigned char* pc = costarr;
        for (int i = 0; i < nx; i++)
            *pc++ = value;
        // 接着，将数组最后一行全部设置为指定的值
        pc = costarr + (ny - 1) * nx;
        for (int i = 0; i < nx; i++)
            *pc++ = value;
        //  // 然后，将数组第一列全部设置为指定的值
        pc = costarr;
        for (int i = 0; i < ny; i++, pc += nx)
            *pc = value;

        // // 最后，将数组最后一列全部设置为指定的值
        pc = costarr + nx - 1;
        for (int i = 0; i < ny; i++, pc += nx)
            *pc = value;

        /**
       pc = costarr + (ny - 1) * nx; 解释一下
       这行代码的作用是让指针 pc 指向数组 costarr 的最后一行的第一个元素。具体而言，由于 costarr 是一个一维数组，按照行优先的顺序存储，因此最后一行的第一个元素的下标应该是 (ny - 1) * nx。
       这里用指针的算术运算 costarr + (ny - 1) * nx 来获得该元素的地址，并将其赋值给指针 pc。这样，接下来就可以通过对 pc 进行递增操作，遍历数组 costarr 的最后一行了。
       */
    }
    //计算起点在一维数组中的下标。// 将起点坐标 start_x 和 start_y 转化为一维数组中的索引 start_i。
    int global_planner::toIndex(int x, int y,int nx ,int ny) {
        return x + nx * y;
    }

    //某个位置在字符数组中的索引为 index，则该位置在地图中的 current_x 和 current_y 像素坐标可以通过以下公式计算：
    void global_planner::toX_Y(int index,int &current_x, int &current_y,int nx ,int ny) {
        current_x = index % nx;
        current_y = index / nx;
    }
    void global_planner::setSize(int nx, int ny) {
        nx_ = nx;
        ny_ = ny;
        ns_ = nx * ny;
    } /**< sets or resets the size of the map */
//获取小车当前位置
    bool global_planner::getRobotPose(geometry_msgs::PoseStamped& global_pose)
    {
        bool  getuwb_tfmapTbase_linkFig=false;
        try {
            //获取矩阵
            geometry_msgs::TransformStamped mapTbase_link;
            string pframe_id ="map";//基坐标
            string cframe_id ="base_link";//base_footprint  base_link
            getuwb_tfmapTbase_linkFig = getuwb_tf(pframe_id,cframe_id,mapTbase_link);

            if(getuwb_tfmapTbase_linkFig){

                global_pose.header.frame_id = mapTbase_link.header.frame_id;
                global_pose.header.stamp = mapTbase_link.header.stamp;
                global_pose.pose.position.x = mapTbase_link.transform.translation.x;
                global_pose.pose.position.y = mapTbase_link.transform.translation.y;
                global_pose.pose.position.z = mapTbase_link.transform.translation.z;
                global_pose.pose.orientation.x = mapTbase_link.transform.rotation.x;
                global_pose.pose.orientation.y = mapTbase_link.transform.rotation.y;
                global_pose.pose.orientation.z = mapTbase_link.transform.rotation.z;
                global_pose.pose.orientation.w = mapTbase_link.transform.rotation.w;
                //std::cout << "=======getRobotPose====获取到机器人当前坐标===================" << std::endl;
                return true;
            } else{
                std::cout << "=======getRobotPose====error 未获取到机器人当前坐标===================" << std::endl;
                return false;
            }

        }catch (exception e){
            std::cout << "=======getRobotPose====异常 未获取到机器人当前坐标===================" << std::endl;
            return false;
        }

    }


    //todo 1.规划路径
    bool global_planner::makePlan(const geometry_msgs::PoseStamped& start, const geometry_msgs::PoseStamped& goal,
                                 std::vector<geometry_msgs::PoseStamped>& plan) {
        return makePlan(start, goal, default_tolerance_, plan);//    1451    private_nh2.param("default_tolerance", default_tolerance_, 0.0);//#默认0.0，目标容差
    }

/**
    std::map<std::string, int> myMap;
// 添加键值对到 myMap

    std::string keyToCheck = "example_key";
    if (containsKey(myMap, keyToCheck)) {
    // 键存在
} else {
// 键不存在
}
*/

//判断map判断key是否存在
    template<typename KeyType, typename ValueType>
    bool global_planner::containsKey(const std::map<KeyType, ValueType>& myMap, const KeyType& keyToCheck) {
        return myMap.find(keyToCheck) != myMap.end();
    }

//todo 2.规划路径
    bool global_planner::makePlan(const geometry_msgs::PoseStamped& start, const geometry_msgs::PoseStamped& goal,
                                 double tolerance, std::vector<geometry_msgs::PoseStamped>& plan) {
        boost::mutex::scoped_lock lock(mutex_);
        if (!initialized_) {
            //此计划器尚未初始化，但正在使用中，请在使用前调用initialize（）
            ROS_ERROR(
                    "This planner has not been initialized yet, but it is being used, please call initialize() before use");
            return false;
        }

        //clear the plan, just in case  清除计划，以防万一
        plan.clear();

        ros::NodeHandle n;
        std::string global_frame = frame_id_;

        std::cout << "1.makePlan 路径规划 global_frame： " <<global_frame<<std::endl;
        std::cout << "2.makePlan 路径规划 goal.header.frame_id： " <<goal.header.frame_id<<std::endl;

        //直到tf能够处理改变过去的事情。。。我们将要求目标在我们的全球框架内
        //until tf can handle transforming things that are way in the past... we'll require the goal to be in our global frame
        if (goal.header.frame_id != global_frame) {
            ROS_ERROR(
                    "The goal pose passed to this planner must be in the %s frame.  It is instead in the %s frame.", global_frame.c_str(), goal.header.frame_id.c_str());
            return false;
        }

        if (start.header.frame_id != global_frame) {
            ROS_ERROR(
                    "The start pose passed to this planner must be in the %s frame.  It is instead in the %s frame.", global_frame.c_str(), start.header.frame_id.c_str());
            return false;
        }

        double wx = start.pose.position.x;
        double wy = start.pose.position.y;
        std::cout << "2.makePlan 起始点坐标 wx： " <<to_string(wx)<<" , wy： " <<to_string(wy)<<std::endl;

        unsigned int start_x_i, start_y_i, goal_x_i, goal_y_i;
        double start_x, start_y, goal_x, goal_y;


        if (!origin_costmap_->worldToMap(wx, wy, start_x_i, start_y_i)) {//世界坐标转像素坐标
            //机器人的起始位置不在全球成本图上。计划总是会失败，你确定机器人已经正确本地化了吗？
            ROS_WARN(
                    "errror 机器人的起始位置不在全球成本图上。计划总是会失败，你确定机器人已经正确本地化了吗 The robot's start position is off the global costmap. Planning will always fail, are you sure the robot has been properly localized?");
            return false;
        }
        std::cout << "3.makePlan 起始点坐标 start_x_i： " <<to_string(start_x_i)<<" , start_y_i： " <<to_string(start_y_i)<<std::endl;

        wx = goal.pose.position.x;
        wy = goal.pose.position.y;
        std::cout << "1.makePlan 终点坐标 wx： " <<to_string(wx)<<" , wy： " <<to_string(wy)<<std::endl;

        if (!origin_costmap_->worldToMap(wx, wy, goal_x_i, goal_y_i)) {
            ROS_WARN_THROTTLE(1.0,
                              "errror 机器人的起始位置不在全球成本图上。计划总是会失败，你确定机器人已经正确本地化了吗 The goal sent to the global planner is off the global costmap. Planning will always fail to this goal.");
            return false;
        }

        std::cout << "4.makePlan 终点坐标 goal_x_i： " <<to_string(goal_x_i)<<" , goal_y_i： " <<to_string(goal_y_i)<<std::endl;


         start_x=(double)start_x_i;
         start_y=(double)start_y_i;
         goal_x=(double)goal_x_i;
         goal_y=(double)goal_y_i;
        std::cout << "5.makePlan 起始点坐标 start_x： " <<to_string(start_x)<<" , start_y： " <<to_string(start_y)<<std::endl;
        std::cout << "6.makePlan 2493 终点坐标 goal_x： " <<to_string(goal_x)<<" , goal_y： " <<to_string(goal_y)<<std::endl;



        //clear the starting cell within the costmap because we know it can't be an obstacle 清除成本图中的起始单元格，因为我们知道它不会成为障碍
        clearRobotCell(start, start_x_i, start_y_i);
        //setSize( cx_,  cy_);
        //make sure to resize the underlying array that Navfn uses 确保调整Navfn使用的基础数组的大小
        p_calc_->setSize(nx_, ny_);
        planner_->setSize(nx_, ny_);
        potential_ = new float[nx_ * ny_]; //地图数组存代价直

        std::cout << "7.makePlan 终点坐标 nx_： " <<to_string(nx_)<<" , ny_： " <<to_string(ny_)<<std::endl;
        /**
        具体而言，该函数的实现方式是，分别用指针 pc 指向数组的第一行、最后一行、第一列和最后一列，
        然后使用循环依次遍历这四个边界，并将它们的每个元素设置为指定的 value 值。
       在代码的实现过程中，nx 和 ny 分别表示数组 costarr 的宽度和高度，unsigned char 是一个无符号字符类型，用
        于存储 8 位无符号整数。因此，该函数的输入参数包括一个指向无符号字符数组的指针 costarr，数组的宽度 nx 和高度 ny，以及要设置的值 value。
         */

        if(outline_map_)
            outlineMap(origin_costmap_->getCharMap(), nx_, ny_, costmap_2d::LETHAL_OBSTACLE);  //costmap_2d::LETHAL_OBSTACLE = 254表示在此单元为会发生碰撞的障碍物。


        ///home/sukai/workspace/xju-robot_gai/src/thirdparty/navigation/global_planner/src/astar.cpp 53
        ///home/sukai/workspace/xju-robot_gai/src/thirdparty/navigation/global_planner/src/dijkstra.cpp 80
        //planner_- 用a* 还是 dijkstra 做全局规划 175
        /**
         * //potential（预估每个点到目标节点的距离）
         * 这段代码是在进行路径规划，其中调用了planner_的calculatePotentials函数，该函数的作用是计算出从起点到终点的所有路径的代价值，即potential_array_数组。其中，costmap_->getCharMap()是获取地图的信息，
         * start_x, start_y, goal_x, goal_y是起点和终点的坐标，nx * ny * 2是数组potential_array_的大小。
         * 函数返回值是一个bool类型的变量found_legal，表示是否找到了一条合法的路径。
         *
         * 在这段代码中，costmap_->getCharMap()返回的是一个unsigned char类型的指针，指向的是一个二维数组，数组中的每个元素表示对应位置的cell的状态。在这个数组中，0表示自由空间，255表示障碍物，其他值表示不同的占用程度。
         * 对不起，我的之前的回答有误。在ROS中，costmap_->getCharMap() 返回的数据类型是指向 unsigned char 类型的一维数组的指针。这个一维数组的长度等于地图的宽度乘以高度。每个数组元素对应着地图中一个单元格，其值表示该单元格的状态。
         * 通常，0 表示自由空间，100 表示障碍物，而其他的值则可以用于表示其他状态。
         *
         * nx * ny * 2
        这个参数是用来指定potential_array_数组的大小的，这个数组是用来存储每个点到目标点的距离的。在这个函数中，potential_array_数组的大小是nx * ny * 2，
         其中nx和ny是地图的宽度和高度，2是因为在计算距离时，需要同时计算正向和反向的距离，因此数组的大小需要乘以2。在这个函数中，nx和ny的值是通过costmap_的getSizeInCellsX()和getSizeInCellsY()函数获取的，
         这两个函数返回的是地图的宽度和高度，单位是cell。因此，nx * ny * 2实际上是地图中所有cell的数量乘以2。
         */
        bool found_legal = planner_->calculatePotentials(origin_costmap_->getCharMap(), start_x, start_y, goal_x, goal_y,
                                                         nx_ * ny_ * 2, potential_);//float* potential_array_; 该函数的作用是计算出从起点到终点的所有路径的代价值，即potential_array_数组

//        if(!old_navfn_behavior_)
//            planner_->clearEndpoint(costmap_->getCharMap(), potential_, goal_x_i, goal_y_i, 2);

        /**
         * 这个函数与getPlanFromPotential(start_x, start_y, goal_x, goal_y, goal, plan)函数的区别在于，getPlanFromPotential函数是用于从潜在代价地图中获取路径的函数，
         * 而publishPotential函数是用于发布潜在代价地图的函数。getPlanFromPotential函数的输入是起点和终点的坐标，
         * 以及一个空的路径plan，函数的输出是一个bool类型的值，表示是否成功获取到路径。如果成功获取到路径，则路径会被存储在plan中。publishPotential函数的输入是一个浮点型数组，
         * 表示每个点到目标节点的距离，函数的输出是一个nav_msgs::OccupancyGrid类型的消息，表示整个代价地图。函数首先获取代价地图的大小和分辨率，然后将整个代价地图发布出去。
         * 在发布代价地图之前，函数会对代价地图进行处理，将代价地图中的每个点的值进行归一化处理，使得代价地图中的最大值为1，最小值为0。最后，函数将处理后的代价地图发布出去。
         */

        if(publish_potential_)
            publishPotential(potential_);

        if (found_legal) {//是否已经计算得到路径代价了
            //extract the plan
            /**
             * 这个函数是用来从potential_array_数组中获取一条从起点到终点的路径的。在这个函数中，start_x和start_y是起点的坐标，goal_x和goal_y是终点的坐标，
             * goal是一个geometry_msgs::PoseStamped类型的变量，表示终点的位置，plan是一个std::vector<geometry_msgs::PoseStamped>类型的变量，用来存储路径上的所有点。在函数中，
             * 首先会调用calculatePotentials()函数计算每个点到终点的距离，然后再从potential_array_ （代价）数组中获取一条从起点到终点的路径，并将路径上的所有点存储到plan变量中。


             */
            if (getPlanFromPotential(potential_,start_x, start_y, goal_x, goal_y, goal, plan)) {
                //make sure the goal we push on has the same timestamp as the rest of the plan 确保我们推进的目标与计划的其余部分具有相同的时间戳
                geometry_msgs::PoseStamped goal_copy = goal;
                goal_copy.header.stamp = ros::Time::now();
                plan.push_back(goal_copy);
            } else {
                ROS_ERROR("error 未能从潜在客户处获取计划Failed to get a plan from potential when a legal potential was found. This shouldn't happen.");//发现合法潜在客户时，未能从潜在客户处获取计划。这不应该发生
            }
        }else{
            ROS_ERROR_THROTTLE(5.0, "error 无法获取计划 Failed to get a plan.");//无法获取计划
        }

        // add orientations if needed 如果需要，添加方向
        orientation_filter_->processPath(start, plan);

        //publish the plan for visualization purposes 出于可视化目的发布计划
        nav_msgs::Path gui_path;
        publishPlan(plan,gui_path);
        plan_pub_.publish(gui_path);
        delete[] potential_;
        return !plan.empty();
    }




    //todo 2.规划路径 半结构路径规划中的 makePlan2 函数
bool global_planner::makePlan2(const geometry_msgs::PoseStamped& start, const geometry_msgs::PoseStamped& goal,
                                  double tolerance, std::vector<geometry_msgs::PoseStamped>& plan) {
        boost::mutex::scoped_lock lock(mutex_);
        if (!initialized_) {
            //此计划器尚未初始化，但正在使用中，请在使用前调用initialize（）
            ROS_ERROR(
                    "This planner has not been initialized yet, but it is being used, please call initialize() before use");
            return false;
        }

        //clear the plan, just in case  清除计划，以防万一
        plan.clear();

        ros::NodeHandle n;
        std::string global_frame = frame_id_;

        std::cout << "1.makePlan2 路径规划 global_frame： " <<global_frame<<std::endl;
        std::cout << "2.makePlan2 路径规划 goal.header.frame_id： " <<goal.header.frame_id<<std::endl;

        //直到tf能够处理改变过去的事情。。。我们将要求目标在我们的全球框架内
        //until tf can handle transforming things that are way in the past... we'll require the goal to be in our global frame
        if (goal.header.frame_id != global_frame) {
            ROS_ERROR(
                    "The goal pose passed to this planner must be in the %s frame.  It is instead in the %s frame.", global_frame.c_str(), goal.header.frame_id.c_str());
            return false;
        }

        if (start.header.frame_id != global_frame) {
            ROS_ERROR(
                    "The start pose passed to this planner must be in the %s frame.  It is instead in the %s frame.", global_frame.c_str(), start.header.frame_id.c_str());
            return false;
        }

        double wx = start.pose.position.x;
        double wy = start.pose.position.y;
        std::cout << "2.makePlan2 起始点坐标 wx： " <<to_string(wx)<<" , wy： " <<to_string(wy)<<std::endl;

        unsigned int start_x_i, start_y_i, goal_x_i, goal_y_i;
        double start_x, start_y, goal_x, goal_y;


        if (!origin_costmap_->worldToMap(wx, wy, start_x_i, start_y_i)) {//世界坐标转像素坐标
            //机器人的起始位置不在全球成本图上。计划总是会失败，你确定机器人已经正确本地化了吗？
            ROS_WARN(
                    "errror 机器人的起始位置不在全球成本图上。计划总是会失败，你确定机器人已经正确本地化了吗 The robot's start position is off the global costmap. Planning will always fail, are you sure the robot has been properly localized?");
            return false;
        }
        std::cout << "3.makePlan 起始点坐标 start_x_i： " <<to_string(start_x_i)<<" , start_y_i： " <<to_string(start_y_i)<<std::endl;

        wx = goal.pose.position.x;
        wy = goal.pose.position.y;
        std::cout << "1.makePlan 终点坐标 wx： " <<to_string(wx)<<" , wy： " <<to_string(wy)<<std::endl;

        if (!origin_costmap_->worldToMap(wx, wy, goal_x_i, goal_y_i)) {
            ROS_WARN_THROTTLE(1.0,
                              "errror 机器人的起始位置不在全球成本图上。计划总是会失败，你确定机器人已经正确本地化了吗 The goal sent to the global planner is off the global costmap. Planning will always fail to this goal.");
            return false;
        }



        std::cout << "4.makePlan2 终点坐标 goal_x_i： " <<to_string(goal_x_i)<<" , goal_y_i： " <<to_string(goal_y_i)<<std::endl;

//        if(outline_map_)
//            outlineMap(origin_costmap_->getCharMap(), nx_, ny_, costmap_2d::LETHAL_OBSTACLE);  //costmap_2d::LETHAL_OBSTACLE = 254表示在此单元为会发生碰撞的障碍物。



        start_x=(double)start_x_i;
        start_y=(double)start_y_i;
        goal_x=(double)goal_x_i;
        goal_y=(double)goal_y_i;
        Point startmone_point ;//
        startmone_point.x=start_x;
        startmone_point.y=start_y;
        Point goalmone_point ;//
        goalmone_point.x=goal_x;
        goalmone_point.y=goal_y;


//        std::cout<<"nx_+++++++++++++++++++++++++++++++++++++++++++++++  nx_     :"<<std::to_string(nx_)<< std::endl;
//        std::cout<<"ny_+++++++++++++++++++++++++++++++++++++++++++++++  ny_     :"<<std::to_string(ny_)<< std::endl;

        int start_index = toIndex(start_x_i, start_y_i,nx_,ny_);//将周围点的坐标转化为数组下标。 toIndex(int x, int y,int nx ,int ny)
        int  goal_index = toIndex(goal_x_i, goal_y_i,nx_,ny_);//将周围点的坐标转化为数组下标。 toIndex(int x, int y,int nx ,int ny)


        std::cout << "5.makePlan2 起始点坐标 start_x： " <<to_string(start_x)<<" , start_y： " <<to_string(start_y)<<std::endl;
        std::cout << "6.makePlan2 终点坐标 goal_x： " <<to_string(goal_x)<<" , goal_y： " <<to_string(goal_y)<<std::endl;

        //封装起点与终点数据
        string startmone_plan_type="start";//路径规划的类型
        string goalmone_plan_type="goal";//路径规划的类型
        int mone_plan_id=-1;//当前路径id，唯一id
        string plan_name="";//当前路径name，随意起名称
        string ns="";//当前路径 namespace ，用当前地图名称+场景编号作命名空间
        std::vector<int> target_plan_ids;//当前路径行走结束后，可以到达的路径id
        std::cout << "7.makePlan2 封装起点与终点数据 " <<std::endl;

        //todo start
        vector<Point>  startmone_distance_plan_one_vector;//todo 保存所有包含转角的路径点，用于计算距离
        startmone_distance_plan_one_vector.push_back(startmone_point);
        std::vector<int> startmone_plan_indexs;//当前路径在地图中的索引,在行优先的一维地图中的索引
        //起始点的索引 start
        int  startmone_index = toIndex((int)startmone_point.x, (int)startmone_point.y,nx_,ny_);//将周围点的坐标转化为数组下标。 toIndex(int x, int y,int nx ,int ny)
        startmone_plan_indexs.push_back(startmone_index);
        std::cout << "8.makePlan2 保存所有包含转角的路径点，用于计算距离 " <<std::endl;
        //todo goal
        vector<Point>  goalmone_distance_plan_noArc_vector;//todo 保存所有包含转角的路径点，用于计算距离
        goalmone_distance_plan_noArc_vector.push_back(goalmone_point);
        std::vector<int> goalmone_plan_indexs;//当前路径在地图中的索引,在行优先的一维地图中的索引
        //目标点的索引 goal
        int  goalmone_index = toIndex((int)goalmone_point.x, (int)goalmone_point.y,nx_,ny_);//将周围点的坐标转化为数组下标。 toIndex(int x, int y,int nx ,int ny)
        goalmone_plan_indexs.push_back(goalmone_index);
        std::cout << "9.makePlan2 保存所有包含转角的路径点，用于计算距离" <<std::endl;
         //todo ============================================初始化参数得到 startmonePlanToMap 与 goalmonePlanToMap 起始点与结束点的数据封装==============================================================================
        MorPlanToMap startmonePlanToMap(startmone_plan_type,mone_plan_id,plan_name,target_plan_ids,startmone_distance_plan_one_vector,startmone_plan_indexs);
        startmonePlanToMap.cost=0;//起始点的cost为0
        MorPlanToMap goalmonePlanToMap (goalmone_plan_type, mone_plan_id,plan_name,target_plan_ids,goalmone_distance_plan_noArc_vector,goalmone_plan_indexs);
        std::cout << "10.makePlan2 初始化参数得到 startmonePlanToMap 与 goalmonePlanToMap 起始点与结束点的数据封装" <<std::endl;
        //end ========start 起始点==========最终目标点goal===============================================================================================
        //clear the starting cell within the costmap because we know it can't be an obstacle 清除成本图中的起始单元格，因为我们知道它不会成为障碍
        clearRobotCell(start, start_x_i, start_y_i);
        //setSize( cx_,  cy_);
        //==================================================================================================================

        std::vector<int> plan_all_indexs = plan_all_indexs_;//todo 当前路径在地图中每个点的索引，每个点在行有优先的地图中的索引
        //todo 1. 判断起始点是否在路径上
        bool is_start_index_in_plan =is_index_in_planFun(start_index,plan_all_indexs);

        if(is_start_index_in_plan){
            std::cout<<"/+++++++++++++++///1: 起始点已经在结构化网格上     :"<<std::to_string(start_index)<< std::endl;
        }
        //todo 2. 判断终点是否在路径上
        bool is_goal_index_in_plan =is_index_in_planFun(goal_index,plan_all_indexs);
        if(is_goal_index_in_plan){
            std::cout<<"/+++++++++++++++///2: 终点已经在结构化网格上     :"<<std::to_string(goal_index)<< std::endl;
        }

//        //打印看数据sm
//        for (int i = 0; i < plan_all_indexs.size(); ++i) {
//            std::cout<<"/+++++++++++++++///3: 所有的索引  plan_all_indexs[i]   :"<<std::to_string(plan_all_indexs[i])<< std::endl;
//
//        }
//        std::cout<<"start+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++"<< std::endl;
//        std::cout<<"/+++++++++++++++///1: start_index     :"<<std::to_string(start_index)<< std::endl;
//        std::cout<<"/+++++++++++++++///2: goal_index     :"<<std::to_string(goal_index)<< std::endl;
//
//        std::cout<<"/+++++++++++++++///1: nx_     :"<<std::to_string(nx_)<< std::endl;
//        std::cout<<"/+++++++++++++++///2: ny_     :"<<std::to_string(ny_)<< std::endl;
//
//        std::cout<<"/+++++++++++++++///1: start_x_i     :"<<std::to_string(start_x_i)<< std::endl;
//        std::cout<<"/+++++++++++++++///2: start_y_i     :"<<std::to_string(start_y_i)<< std::endl;
//
//        std::cout<<"/+++++++++++++++///1: goal_x_i     :"<<std::to_string(goal_x_i)<< std::endl;
//        std::cout<<"/+++++++++++++++///2: goal_y_i     :"<<std::to_string(goal_y_i)<< std::endl;
//        std::cout<<"end+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++"<< std::endl;


        //不在路径上的操作
        std::cout << "11.makePlan2 找到起始点与结束点的切入点" <<std::endl;
        //todo 2.找到起始点与结束点的切入点。 start_x start_y 最近的路径； goal_x goal_y 最近的路径
        //-----------------------------//把所有半结构化路径在地图中的索引重新放在一个容器里-------------------------------------------------------------------
//        //起点
//        std::vector<PointDistance> distance_vector;//储存起点到各路径所有点的曼哈顿距离的容器
//        //终点
//        std::vector<PointDistance> distance_goal_vector;//储存终点到各路径所有点的曼哈顿距离的容器

        //切入点起点
        map<int, PointDistance> distance_start_map;//储存起点到各路径所有点的曼哈顿距离的容器
        //切出点终点
        map<int, PointDistance> distance_goal_map;//储存终点到各路径所有点的曼哈顿距离的容器

        //---
        map<int, MorPlanToMap>::iterator iter;
        //通过每条路径的索引得到像素坐标
        //todo 1.从半结构化每条路径中的所有点的中找最小曼哈顿距离的点位置【每条路径都对应一个可到达的（距曼哈顿离最短）点位】 [储存所有到路径点的起点距离数据 distance_vector] [储存所有到路径点的终点距离数据 distance_goal_vector]
        for(iter = execute_map_plan_.begin(); iter != execute_map_plan_.end(); iter++) {
            int current_plan_id=iter->first; //todo 当前路径id，唯一id ；每个路径在数据库中都会存储一个唯一的id
            MorPlanToMap current_morPlanToMap=  iter->second;//todo 当前的路径
            std::vector<int> plan_indexs = current_morPlanToMap.plan_indexs;//todo 当前路径在地图中每个点的索引，每个点在行有优先的地图中的索引
            std::vector<Point> distance_plan_all_point = current_morPlanToMap.distance_plan_all_vector;//todo 路径,一条路径的所有点的像素坐标
            double start_minDistance=POT_HIGH;//todo 在一条路径上所有点的最小曼哈顿距离
            double goal_minDistance=POT_HIGH; //todo 在一条路径上所有点的最小曼哈顿距离
            int index_start_min_i=-1;//std::vector<Point> distance_plan_all_point  的索引，通过这个索引可以得到 路径的唯一id ： plan_indexs[index_start_min_i]； std::vector<int> plan_indexs
            int index_goal_min_i=-1;

            int plan_index_start_min=-1;//每个点在地图中的索引
            int plan_index_goal_min=-1;//每个点在地图中的索引

            Point index_start_min_point ;// 当前点位起点最近的点
            Point index_goal_min_point ;// 当前点位终点最近的点

              //todo 1. 判断起始点,判断终点是否在路径上
            if(is_start_index_in_plan||is_goal_index_in_plan) {// 起始点或终点 在 路径上

                if(is_start_index_in_plan){
                    //判断起始点是否在当前的路径上,并获得列表对应的id
                    int  start_result_index;//返回的在vector列表de索引
                    bool start_index_is_index_in_planfig =is_index_in_planFun(start_index,plan_indexs,start_result_index);

                    if(start_index_is_index_in_planfig){//起始点是在路径上
                        Point point = distance_plan_all_point[start_result_index];//todo 像素点
                        int plan_index = plan_indexs[start_result_index];//todo 每个点在行优先的地图中的索引，像素点对应的索引

                        start_minDistance=0;//计算起点到当前路径所有点的曼哈顿距离
                        index_start_min_i=start_result_index; //std::vector<Point> distance_plan_all_point, vector集合的id  ； 通过这个索引可以得到 路径的唯一id ： plan_indexs[index_start_min_i]； std::vector<int> plan_indexs
                        index_start_min_point=point;//当前点位起点最近的点
                        plan_index_start_min=plan_index;//每个点在行有优先的地图中的索引
                    }
                }
                //------------------------------------------------------------
                if(is_goal_index_in_plan){
                    //判断终点点是否在当前的路径上,并获得列表对应的id
                    int  goal_result_index;//返回的在vector列表de索引
                    bool goal_index_is_index_in_planfig =is_index_in_planFun(goal_index,plan_indexs,goal_result_index);

                    if(goal_index_is_index_in_planfig){//终点是在路径上
                        Point point = distance_plan_all_point[goal_result_index];//todo 像素点
                        int plan_index = plan_indexs[goal_result_index];//todo 每个点在行优先的地图中的索引，像素点对应的索引
                        goal_minDistance=0;//计算终点点到当前路径所有点的曼哈顿距离
                        index_goal_min_i=goal_result_index;//vector集合的id  ；  std::vector<Point> distance_plan_all_point  的索引，通过这个索引可以得到 路径的唯一id ： plan_indexs[index_start_min_i]； std::vector<int> plan_indexs
                        index_goal_min_point=point;//当前点位终点最近的点
                        plan_index_goal_min=plan_index;//每个点在行有优先的地图中的索引
                    }
                }


            }//end  if(is_start_index_in_plan||is_goal_index_in_plan) {// 起始点,终点 在 路径上


           // 起始点,终点 不在 路径上
           if(!is_start_index_in_plan||!is_goal_index_in_plan){// 起始点,终点 不在 路径上

                    for (int i = 0; i < distance_plan_all_point.size(); ++i) {//distance_plan_all_point 路径,一条路径的所有点的像素坐标
                        Point point = distance_plan_all_point[i];//todo 像素点
                        int plan_index = plan_indexs[i];//todo 每个点在行优先的地图中的索引，像素点对应的索引

                        double start_distance = abs(start_x - point.x) + abs(start_y - point.y);//计算起点到当前路径所有点的曼哈顿距离。
                        double goal_distance  = abs(goal_x - point.x) + abs(goal_y - point.y);//计算终点到当前路径所有点的曼哈顿距离。
                        if(!is_start_index_in_plan){//起始点不在路径上
                            if(start_distance<start_minDistance){
                                start_minDistance=start_distance;//计算起点到当前路径所有点的曼哈顿距离
                                index_start_min_i=i; //std::vector<Point> distance_plan_all_point, vector集合的id  ； 通过这个索引可以得到 路径的唯一id ： plan_indexs[index_start_min_i]； std::vector<int> plan_indexs
                                index_start_min_point=point;//当前点位起点最近的点
                                plan_index_start_min=plan_index;//每个点在行有优先的地图中的索引
                            }
                        }
                        if(!is_goal_index_in_plan) {//终点不在路径上
                            if(goal_distance<goal_minDistance){
                                goal_minDistance=goal_distance;//计算终点点到当前路径所有点的曼哈顿距离
                                index_goal_min_i=i;//vector集合的id  ；  std::vector<Point> distance_plan_all_point  的索引，通过这个索引可以得到 路径的唯一id ： plan_indexs[index_start_min_i]； std::vector<int> plan_indexs
                                index_goal_min_point=point;//当前点位终点最近的点
                                plan_index_goal_min=plan_index;//每个点在行有优先的地图中的索引
                            }

                        }

                    }

                }//end if(!is_start_index_in_plan||!is_goal_index_in_plan){// odo 3.起始点,终点 不在 路径上


            //plan_indexs[i]:每个点在地图中的索引 ,index_goal_min_i=distance_plan_all_point_index 当前点在distance_plan_all_point,  start_distance:起点到当前路径所有点的曼哈顿距离。,goal_distance:终点到当前路径所有点的曼哈顿距离。point:点的像素坐标
            if(index_start_min_i!=-1){
            PointDistance index_start_min_pointDistance(plan_index_start_min,index_start_min_i,start_minDistance,goal_minDistance,index_start_min_point,current_morPlanToMap.plan_id,current_morPlanToMap.plan_name,current_morPlanToMap.ns);
            //distance_vector.push_back(index_start_min_pointDistance);//储存起点到各路径所有点的曼哈顿距离的容器
            distance_start_map[current_morPlanToMap.plan_id]=index_start_min_pointDistance;//储存起点到各路径所有点的曼哈顿距离的容器

            }
            if(index_goal_min_i!=-1){ //plan_index_goal_min ，index_goal_min_i=distance_plan_all_point_index 当前点在distance_plan_all_point, vector集合中的索引 ：每个点在地图中的索引，index_goal_min_i  ，start_minDistance：起点到当前路径所有点的曼哈顿距离,goal_minDistance：终点点到当前路径所有点的曼哈顿距离    ，index_goal_min_point：当前点位终点最近的点，plan_id 路径id，plan_name路径名称，ns路径的命名空间
            PointDistance index_goal_min_pointDistance(plan_index_goal_min, index_goal_min_i,start_minDistance,goal_minDistance,index_goal_min_point, current_morPlanToMap.plan_id,current_morPlanToMap.plan_name,current_morPlanToMap.ns);
            //distance_goal_vector.push_back(index_goal_min_pointDistance);//储存终点到各路径所有点的曼哈顿距离的容器
            distance_goal_map[current_morPlanToMap.plan_id]=index_goal_min_pointDistance;//储存终点到各路径所有点的曼哈顿距离的容器
            }

        }// end for(iter = execute_map_plan_.begin(); iter != execute_map_plan_.end(); iter++)




        //找到起始切入点 distance_vector =》每条路径的一个最小距离点位  ； 找到起始切入点 distance_goal_vector =》每条路径的一个最小距离点位

        std::cout << "12.makePlan2 从每条路径中 找到起点与终点 对应一个可到达的（距曼哈顿离最短）点位最近的一条路径 || 找到起始切入点 distance_vector =》每条路径的一个最小距离点位  ； 找到起始切入点 distance_goal_vector =》每条路径的一个最小距离点位" <<std::endl;


        if(distance_start_map.size() == 0||distance_goal_map.size() == 0 ){
            cout<<"3. 找到起始点与结束点的最近切入点 未找到,if(distance_start_map.size() == 0||distance_goal_map.size() == 0 )"<<endl;
            return false;
        }
        cout<<"4. 找到起始点与结束点的最近切入点 distance_start_map.size() == 0: "<<to_string(distance_start_map.size())<<"   ;distance_goal_map.size(): "<<to_string(distance_goal_map.size())<<endl;


         //todo **************************** 广度优先搜索（BFS）***********************************************************
        // start_min_plan_id;////找到 起始切入路径唯一id
        //goal_min_plan_id;////找到 目标切切出点径唯一id
        std::vector<std::vector<int>> all_paths; // 存储所有路径; [sukai 提取出来] 存储所有路径的id
        string bfs_result_message;// 广度优先搜索（BFS）结果信息
        //// 可以使用广度优先搜索（BFS）算法来找出起始点到目标点的所有路径，并计算每条路径的长度，然后选择路径长度最短的路径作为结果输出。  start_min_plan_id:起始切入点在的 路径id 对应的路径 作为开始路径；goal_min_plan_id:切出点在 路径id对应的路径，作为结束路径    , start_min_index 切入点在在行有优先的地图中的索引 ，goal_min_index 切出点在在在行有优先的地图中的索引
       bool find_shortest_pathFig = find_shortest_path( distance_start_map, distance_goal_map, execute_map_plan_,all_paths,bfs_result_message);

       if(find_shortest_pathFig == false) {
           cout << "4. 广度优先搜索（BFS）未找到路径" << endl;
           return false;
       }
        //=========================================================================
        std::cout << "15.makePlan2 获取半结构全局规划路径,path_id:路径的id,int start_min_index 起始切入路径索引,int goal_min_index:目标切切出点; Point &start_point 起点点位,Point &goal_point终点点位）" <<std::endl;



//        PointDistance start_min_pointDistance ;//todo 找到 起始切入点
//        PointDistance goal_min_pointDistance ;//todo  找到 目标切切出点

        //todo ***********获取半结构全局规划路径,path_id:路径的id,int start_min_index 起始切入路径索引,int goal_min_index:目标切切出点; Point &start_point 起点点位,Point &goal_point终点点位
        // start_min_index ，goal_min_index每个点在行有优先的地图中的索引  //        //获取半结构全局规划路径,path_id:路径的id,, PointDistance &start_min_pointDistance ： 起始切入点,PointDistance &goal_min_pointDistance：目标切切出点; Point &startmone_point 起点点位,Point &goalmone_point终点点位，//找到起始点的最近切入点-，找到 start_x start_y最近的路径对应的点位 Point start_min_point  ；
     //   bool getShortestPathfig =getShortestPath(all_paths,execute_map_plan_,start_min_pointDistance,goal_min_pointDistance,startmone_point,goalmone_point,plan);
        string shortestPath_result_message;//获取半结构全局规划路径 结果信息
        bool getShortestPathfig =getShortestPath(all_paths,execute_map_plan_, distance_start_map,distance_goal_map,startmone_point,goalmone_point,plan,shortestPath_result_message,current_path_goTo_target_path_map_plan_);

            if(getShortestPathfig == false){
            cout<<"16. error 获取半结构全局规划路径 失败！ getShortestPath"<<endl;
            return false;
        }

        std::cout << "17.makePlan2 add orientations if needed 如果需要，添加方向）" <<std::endl;

//----------------------------------------------------------------------

        // add orientations if needed 如果需要，添加方向
     //   orientation_filter_->processPath(start, plan);//std::vector<geometry_msgs::PoseStamped>& plan
        std::cout << "17.makePlan2 出于可视化目的发布计划" <<std::endl;
        for (int i = 0; i < plan.size(); ++i) {//加方向
            if(i < plan.size() - 1)
               calculate_direction(plan[i+1], plan[i]);
        }



        //publish the plan for visualization purposes 出于可视化目的发布计划
        nav_msgs::Path gui_path;
        publishPlan(plan,gui_path);
        pathPlan_semiStructured_pub_.publish(gui_path);
        std::cout << "18.makePlan2 end 出于可视化目的发布计划" <<std::endl;

        return !plan.empty();//makePlan2
    }


    //todo 2.规划路径 半结构路径规划中的 makePlan2Overturn 函数 翻转路径
bool global_planner::makePlan2Overturn(const geometry_msgs::PoseStamped& start, const geometry_msgs::PoseStamped& goal,
                                   double tolerance, std::vector<geometry_msgs::PoseStamped>& plan) {
        boost::mutex::scoped_lock lock(mutex_);
        if (!initialized_) {
            //此计划器尚未初始化，但正在使用中，请在使用前调用initialize（）
            ROS_ERROR(
                    "This planner has not been initialized yet, but it is being used, please call initialize() before use");
            return false;
        }

        //clear the plan, just in case  清除计划，以防万一
        plan.clear();

        ros::NodeHandle n;
        std::string global_frame = frame_id_;

        std::cout << "1.makePlan2Overturn 路径规划 global_frame： " <<global_frame<<std::endl;
        std::cout << "2.makePlan2Overturn 路径规划 goal.header.frame_id： " <<goal.header.frame_id<<std::endl;

        //直到tf能够处理改变过去的事情。。。我们将要求目标在我们的全球框架内
        //until tf can handle transforming things that are way in the past... we'll require the goal to be in our global frame
        if (goal.header.frame_id != global_frame) {
            ROS_ERROR(
                    "The goal pose passed to this planner must be in the %s frame.  It is instead in the %s frame.", global_frame.c_str(), goal.header.frame_id.c_str());
            return false;
        }

        if (start.header.frame_id != global_frame) {
            ROS_ERROR(
                    "The start pose passed to this planner must be in the %s frame.  It is instead in the %s frame.", global_frame.c_str(), start.header.frame_id.c_str());
            return false;
        }

        double wx = start.pose.position.x;
        double wy = start.pose.position.y;
        std::cout << "2.makePlan2Overturn 起始点坐标 wx： " <<to_string(wx)<<" , wy： " <<to_string(wy)<<std::endl;

        unsigned int start_x_i, start_y_i, goal_x_i, goal_y_i;
        double start_x, start_y, goal_x, goal_y;


        if (!origin_costmap_->worldToMap(wx, wy, start_x_i, start_y_i)) {//世界坐标转像素坐标
            //机器人的起始位置不在全球成本图上。计划总是会失败，你确定机器人已经正确本地化了吗？
            ROS_WARN(
                    "errror 机器人的起始位置不在全球成本图上。计划总是会失败，你确定机器人已经正确本地化了吗 The robot's start position is off the global costmap. Planning will always fail, are you sure the robot has been properly localized?");
            return false;
        }
        std::cout << "3.makePlan2Overturn 起始点坐标 start_x_i： " <<to_string(start_x_i)<<" , start_y_i： " <<to_string(start_y_i)<<std::endl;

        wx = goal.pose.position.x;
        wy = goal.pose.position.y;
        std::cout << "4.makePlan2Overturn 终点坐标 wx： " <<to_string(wx)<<" , wy： " <<to_string(wy)<<std::endl;

        if (!origin_costmap_->worldToMap(wx, wy, goal_x_i, goal_y_i)) {
            ROS_WARN_THROTTLE(1.0,
                              "errror 机器人的起始位置不在全球成本图上。计划总是会失败，你确定机器人已经正确本地化了吗 The goal sent to the global planner is off the global costmap. Planning will always fail to this goal.");
            return false;
        }



        std::cout << "4.makePlan2Overturn 终点坐标 goal_x_i： " <<to_string(goal_x_i)<<" , goal_y_i： " <<to_string(goal_y_i)<<std::endl;

//        if(outline_map_)
//            outlineMap(origin_costmap_->getCharMap(), nx_, ny_, costmap_2d::LETHAL_OBSTACLE);  //costmap_2d::LETHAL_OBSTACLE = 254表示在此单元为会发生碰撞的障碍物。



        start_x=(double)start_x_i;
        start_y=(double)start_y_i;
        goal_x=(double)goal_x_i;
        goal_y=(double)goal_y_i;
        Point startmone_point ;//
        startmone_point.x=start_x;
        startmone_point.y=start_y;
        Point goalmone_point ;//
        goalmone_point.x=goal_x;
        goalmone_point.y=goal_y;

        int start_index = toIndex(start_x_i, start_y_i,nx_,ny_);//将周围点的坐标转化为数组下标。 toIndex(int x, int y,int nx ,int ny)
        int  goal_index = toIndex(goal_x_i, goal_y_i,nx_,ny_);//将周围点的坐标转化为数组下标。 toIndex(int x, int y,int nx ,int ny)


        std::cout << "5.makePlan2Overturn 起始点坐标 start_x： " <<to_string(start_x)<<" , start_y： " <<to_string(start_y)<<std::endl;
        std::cout << "6.makePlan2Overturn 终点坐标 goal_x： " <<to_string(goal_x)<<" , goal_y： " <<to_string(goal_y)<<std::endl;

        //封装起点与终点数据
        string startmone_plan_type="start";//路径规划的类型
        string goalmone_plan_type="goal";//路径规划的类型
        int mone_plan_id=-1;//当前路径id，唯一id
        string plan_name="";//当前路径name，随意起名称
        string ns="";//当前路径 namespace ，用当前地图名称+场景编号作命名空间
        std::vector<int> target_plan_ids;//当前路径行走结束后，可以到达的路径id
        std::cout << "7.makePlan2Overturn 封装起点与终点数据 " <<std::endl;

        //todo start
        vector<Point>  startmone_distance_plan_one_vector;//todo 保存所有包含转角的路径点，用于计算距离
        startmone_distance_plan_one_vector.push_back(startmone_point);
        std::vector<int> startmone_plan_indexs;//当前路径在地图中的索引,在行优先的一维地图中的索引
        //起始点在地图像素的索引 start
        int  startmone_index = toIndex((int)startmone_point.x, (int)startmone_point.y,nx_,ny_);//将周围点的坐标转化为数组下标。 toIndex(int x, int y,int nx ,int ny)
        startmone_plan_indexs.push_back(startmone_index);
        std::cout << "8.makePlan2Overturn 保存所有包含转角的路径点，用于计算距离 " <<std::endl;
        //todo goal
        vector<Point>  goalmone_distance_plan_noArc_vector;//todo 保存所有包含转角的路径点，用于计算距离
        goalmone_distance_plan_noArc_vector.push_back(goalmone_point);
        std::vector<int> goalmone_plan_indexs;//当前路径在地图中的索引,在行优先的一维地图中的索引
        //目标点的索引 goal
        int  goalmone_index = toIndex((int)goalmone_point.x, (int)goalmone_point.y,nx_,ny_);//将周围点的坐标转化为数组下标。 toIndex(int x, int y,int nx ,int ny)
        goalmone_plan_indexs.push_back(goalmone_index);
        std::cout << "9.makePlan2Overturn 保存所有包含转角的路径点，用于计算距离" <<std::endl;
        //todo ============================================初始化参数得到 startmonePlanToMap 与 goalmonePlanToMap 起始点与结束点的数据封装==============================================================================
        MorPlanToMap startmonePlanToMap(startmone_plan_type,mone_plan_id,plan_name,target_plan_ids,startmone_distance_plan_one_vector,startmone_plan_indexs);
        startmonePlanToMap.cost=0;//起始点的cost为0
        MorPlanToMap goalmonePlanToMap (goalmone_plan_type, mone_plan_id,plan_name,target_plan_ids,goalmone_distance_plan_noArc_vector,goalmone_plan_indexs);
        std::cout << "10.makePlan2Overturn 初始化参数得到 startmonePlanToMap 与 goalmonePlanToMap 起始点与结束点的数据封装" <<std::endl;
        //end ========start 起始点==========最终目标点goal===============================================================================================
        //clear the starting cell within the costmap because we know it can't be an obstacle 清除成本图中的起始单元格，因为我们知道它不会成为障碍
        clearRobotCell(start, start_x_i, start_y_i);
       // setSize( cx_,  cy_);
        //==================================================================================================================

        std::vector<int> plan_all_indexs = plan_all_indexs_;//todo 当前路径在地图中每个点的索引，每个点在行有优先的地图中的索引
        //todo 1. 判断起始点是否在路径上
        bool is_start_index_in_plan =is_index_in_planFun(start_index,plan_all_indexs);

        if(is_start_index_in_plan){
            std::cout<<"/+++++++++++++++///1: 起始点已经在结构化网格上     :"<<std::to_string(start_index)<< std::endl;
        }
        //todo 2. 判断终点是否在路径上
        bool is_goal_index_in_plan =is_index_in_planFun(goal_index,plan_all_indexs);
        if(is_goal_index_in_plan){
            std::cout<<"/+++++++++++++++///2: 终点已经在结构化网格上     :"<<std::to_string(goal_index)<< std::endl;
        }
//        //打印看数据sm
//        for (int i = 0; i < plan_all_indexs.size(); ++i) {
//            std::cout<<"/+++++++++++++++///3: 所有的索引  plan_all_indexs[i]   :"<<std::to_string(plan_all_indexs[i])<< std::endl;
//
//        }
//        std::cout<<"start+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++"<< std::endl;
//        std::cout<<"/+++++++++++++++///1: start_index     :"<<std::to_string(start_index)<< std::endl;
//        std::cout<<"/+++++++++++++++///2: goal_index     :"<<std::to_string(goal_index)<< std::endl;
//
//        std::cout<<"/+++++++++++++++///1: nx_     :"<<std::to_string(nx_)<< std::endl;
//        std::cout<<"/+++++++++++++++///2: ny_     :"<<std::to_string(ny_)<< std::endl;
//
//        std::cout<<"/+++++++++++++++///1: start_x_i     :"<<std::to_string(start_x_i)<< std::endl;
//        std::cout<<"/+++++++++++++++///2: start_y_i     :"<<std::to_string(start_y_i)<< std::endl;
//
//        std::cout<<"/+++++++++++++++///1: goal_x_i     :"<<std::to_string(goal_x_i)<< std::endl;
//        std::cout<<"/+++++++++++++++///2: goal_y_i     :"<<std::to_string(goal_y_i)<< std::endl;
//        std::cout<<"end+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++"<< std::endl;



        //不在路径上的操作
        std::cout << "11.makePlan2Overturn 找到起始点与结束点的切入点" <<std::endl;
        //todo 2.找到起始点与结束点的切入点。 start_x start_y 最近的路径； goal_x goal_y 最近的路径
        //-----------------------------//把所有半结构化路径在地图中的索引重新放在一个容器里-------------------------------------------------------------------
//        //起点
//        std::vector<PointDistance> distance_vector;//储存起点到各路径所有点的曼哈顿距离的容器
//        //终点
//        std::vector<PointDistance> distance_goal_vector;//储存终点到各路径所有点的曼哈顿距离的容器

        //切入点起点
        map<int, PointDistance> distance_start_map;//储存起点到各路径所有点的曼哈顿距离的容器
        //切出点终点
        map<int, PointDistance> distance_goal_map;//储存终点到各路径所有点的曼哈顿距离的容器

        //---
        map<int, MorPlanToMap>::iterator iter;
        //通过每条路径的索引得到像素坐标
        //todo 1.从半结构化每条路径中的所有点的中找最小曼哈顿距离的点位置【每条路径都对应一个可到达的（距曼哈顿离最短）点位】 [储存所有到路径点的起点距离数据 distance_vector] [储存所有到路径点的终点距离数据 distance_goal_vector]
        for(iter = execute_map_plan_Overturn_.begin(); iter != execute_map_plan_Overturn_.end(); iter++) {
            int current_plan_id=iter->first; //todo 当前路径id，唯一id ；每个路径在数据库中都会存储一个唯一的id
            MorPlanToMap current_morPlanToMap=  iter->second;//todo 当前的路径
            std::vector<int> plan_indexs = current_morPlanToMap.plan_indexs;//todo 当前路径在地图中每个点的索引，每个点在行有优先的地图中的索引
            std::vector<Point> distance_plan_all_point = current_morPlanToMap.distance_plan_all_vector;//todo 路径,一条路径的所有点的像素坐标
            double start_minDistance=POT_HIGH;//todo 在一条路径上所有点的最小曼哈顿距离
            double goal_minDistance=POT_HIGH; //todo 在一条路径上所有点的最小曼哈顿距离
            int index_start_min_i=-1;//std::vector<Point> distance_plan_all_point  的索引，通过这个索引可以得到 路径的唯一id ： plan_indexs[index_start_min_i]； std::vector<int> plan_indexs
            int index_goal_min_i=-1;

            int plan_index_start_min=-1;//每个点在地图中的索引
            int plan_index_goal_min=-1;//每个点在地图中的索引

            Point index_start_min_point ;// 当前点位起点最近的点
            Point index_goal_min_point ;// 当前点位终点最近的点

            //todo 1. 判断起始点,判断终点是否在路径上
            if(is_start_index_in_plan||is_goal_index_in_plan) {// 起始点或终点 在 路径上

                if(is_start_index_in_plan){
                    //判断起始点是否在当前的路径上,并获得列表对应的id
                    int  start_result_index;//返回的在vector列表de索引
                    bool start_index_is_index_in_planfig =is_index_in_planFun(start_index,plan_indexs,start_result_index);

                    if(start_index_is_index_in_planfig){//起始点是在路径上
                        Point point = distance_plan_all_point[start_result_index];//todo 像素点
                        int plan_index = plan_indexs[start_result_index];//todo 每个点在行优先的地图中的索引，像素点对应的索引

                        start_minDistance=0;//计算起点到当前路径所有点的曼哈顿距离
                        index_start_min_i=start_result_index; //std::vector<Point> distance_plan_all_point, vector集合的id  ； 通过这个索引可以得到 路径的唯一id ： plan_indexs[index_start_min_i]； std::vector<int> plan_indexs
                        index_start_min_point=point;//当前点位起点最近的点
                        plan_index_start_min=plan_index;//每个点在行有优先的地图中的索引
                    }
                }
                //------------------------------------------------------------
                if(is_goal_index_in_plan){
                    //判断终点点是否在当前的路径上,并获得列表对应的id
                    int  goal_result_index;//返回的在vector列表de索引
                    bool goal_index_is_index_in_planfig =is_index_in_planFun(goal_index,plan_indexs,goal_result_index);

                    if(goal_index_is_index_in_planfig){//终点是在路径上
                        Point point = distance_plan_all_point[goal_result_index];//todo 像素点
                        int plan_index = plan_indexs[goal_result_index];//todo 每个点在行优先的地图中的索引，像素点对应的索引
                        goal_minDistance=0;//计算终点点到当前路径所有点的曼哈顿距离
                        index_goal_min_i=goal_result_index;//vector集合的id  ；  std::vector<Point> distance_plan_all_point  的索引，通过这个索引可以得到 路径的唯一id ： plan_indexs[index_start_min_i]； std::vector<int> plan_indexs
                        index_goal_min_point=point;//当前点位终点最近的点
                        plan_index_goal_min=plan_index;//每个点在行有优先的地图中的索引
                    }
                }


            }//end  if(is_start_index_in_plan||is_goal_index_in_plan) {// 起始点,终点 在 路径上


            // 起始点,终点 不在 路径上
            if(!is_start_index_in_plan||!is_goal_index_in_plan){// 起始点,终点 不在 路径上

                for (int i = 0; i < distance_plan_all_point.size(); ++i) {//distance_plan_all_point 路径,一条路径的所有点的像素坐标
                    Point point = distance_plan_all_point[i];//todo 像素点
                    int plan_index = plan_indexs[i];//todo 每个点在行优先的地图中的索引，像素点对应的索引

                    double start_distance = abs(start_x - point.x) + abs(start_y - point.y);//计算起点到当前路径所有点的曼哈顿距离。
                    double goal_distance  = abs(goal_x - point.x) + abs(goal_y - point.y);//计算终点到当前路径所有点的曼哈顿距离。
                    if(!is_start_index_in_plan){//起始点不在路径上
                        if(start_distance<start_minDistance){
                            start_minDistance=start_distance;//计算起点到当前路径所有点的曼哈顿距离
                            index_start_min_i=i; //std::vector<Point> distance_plan_all_point, vector集合的id  ； 通过这个索引可以得到 路径的唯一id ： plan_indexs[index_start_min_i]； std::vector<int> plan_indexs
                            index_start_min_point=point;//当前点位起点最近的点
                            plan_index_start_min=plan_index;//每个点在行有优先的地图中的索引
                        }
                    }
                    if(!is_goal_index_in_plan) {//终点不在路径上
                        if(goal_distance<goal_minDistance){
                            goal_minDistance=goal_distance;//计算终点点到当前路径所有点的曼哈顿距离
                            index_goal_min_i=i;//vector集合的id  ；  std::vector<Point> distance_plan_all_point  的索引，通过这个索引可以得到 路径的唯一id ： plan_indexs[index_start_min_i]； std::vector<int> plan_indexs
                            index_goal_min_point=point;//当前点位终点最近的点
                            plan_index_goal_min=plan_index;//每个点在行有优先的地图中的索引
                        }

                    }

                }

            }//end if(!is_start_index_in_plan||!is_goal_index_in_plan){// odo 3.起始点,终点 不在 路径上


            //plan_indexs[i]:每个点在地图中的索引 ,index_goal_min_i=distance_plan_all_point_index 当前点在distance_plan_all_point,  start_distance:起点到当前路径所有点的曼哈顿距离。,goal_distance:终点到当前路径所有点的曼哈顿距离。point:点的像素坐标
            if(index_start_min_i!=-1){
                PointDistance index_start_min_pointDistance(plan_index_start_min,index_start_min_i,start_minDistance,goal_minDistance,index_start_min_point,current_morPlanToMap.plan_id,current_morPlanToMap.plan_name,current_morPlanToMap.ns);
                //distance_vector.push_back(index_start_min_pointDistance);//储存起点到各路径所有点的曼哈顿距离的容器
                distance_start_map[current_morPlanToMap.plan_id]=index_start_min_pointDistance;//储存起点到各路径所有点的曼哈顿距离的容器

            }
            if(index_goal_min_i!=-1){ //plan_index_goal_min ，index_goal_min_i=distance_plan_all_point_index 当前点在distance_plan_all_point, vector集合中的索引 ：每个点在地图中的索引，index_goal_min_i  ，start_minDistance：起点到当前路径所有点的曼哈顿距离,goal_minDistance：终点点到当前路径所有点的曼哈顿距离    ，index_goal_min_point：当前点位终点最近的点，plan_id 路径id，plan_name路径名称，ns路径的命名空间
                PointDistance index_goal_min_pointDistance(plan_index_goal_min, index_goal_min_i,start_minDistance,goal_minDistance,index_goal_min_point, current_morPlanToMap.plan_id,current_morPlanToMap.plan_name,current_morPlanToMap.ns);
                //distance_goal_vector.push_back(index_goal_min_pointDistance);//储存终点到各路径所有点的曼哈顿距离的容器
                distance_goal_map[current_morPlanToMap.plan_id]=index_goal_min_pointDistance;//储存终点到各路径所有点的曼哈顿距离的容器
            }

        }// end for(iter = execute_map_plan_.begin(); iter != execute_map_plan_.end(); iter++)




        //=======================================================================


        //找到起始切入点 distance_vector =》每条路径的一个最小距离点位  ； 找到起始切入点 distance_goal_vector =》每条路径的一个最小距离点位

        std::cout << "12.makePlan2Overturn 从每条路径中 找到起点与终点 对应一个可到达的（距曼哈顿离最短）点位最近的一条路径 || 找到起始切入点 distance_vector =》每条路径的一个最小距离点位  ； 找到起始切入点 distance_goal_vector =》每条路径的一个最小距离点位" <<std::endl;

        //---
        //todo 从每条路径中 找到起点与终点 对应一个可到达的（距曼哈顿离最短）点位最近的一条路径，找到起始切入点 distance_vector =》每条路径的一个最小距离点位  ； 找到起始切入点 distance_goal_vector =》每条路径的一个最小距离点位



        //end---------------------结束点的最近切出点----------------------------

//         if(distance_vector.size() == 0||distance_goal_vector.size() == 0 ){
//                cout<<"3. 找到起始点与结束点的最近切入点 未找到"<<endl;
//                return false;
//         }
        //start_min_plan_id:起始切入点在的 路径id 对应的路径 作为开始路径；goal_min_plan_id:切出点在 路径id对应的路径，作为结束路径 ； start_min_index 切入点在在行有优先的地图中的索引 ，goal_min_index 切出点在在在行有优先的地图中的索引
//        if(start_min_plan_id == -1||goal_min_plan_id == -1||start_min_index == -1||goal_min_index == -1 ){
//            cout<<"3. 找到起始点与结束点的最近切入点 未找到,if(start_min_plan_id == -1||goal_min_plan_id == -1||start_min_index == -1||goal_min_index == -1 )"<<endl;
//            return false;
//        }

        if(distance_start_map.size() == 0||distance_goal_map.size() == 0 ){
            cout<<"3. 找到起始点与结束点的最近切入点 未找到,if(distance_start_map.size() == 0||distance_goal_map.size() == 0 )"<<endl;
            return false;
        }
        cout<<"4. 找到起始点与结束点的最近切入点 distance_start_map.size() == 0: "<<to_string(distance_start_map.size())<<"   ;distance_goal_map.size(): "<<to_string(distance_goal_map.size())<<endl;


        //todo **************************** 广度优先搜索（BFS）***********************************************************
        // start_min_plan_id;////找到 起始切入路径唯一id
        //goal_min_plan_id;////找到 目标切切出点径唯一id
        std::vector<std::vector<int>> all_paths; // 存储所有路径; [sukai 提取出来] 存储所有路径的id
        string bfs_result_message;// 广度优先搜索（BFS）结果信息
        //// 可以使用广度优先搜索（BFS）算法来找出起始点到目标点的所有路径，并计算每条路径的长度，然后选择路径长度最短的路径作为结果输出。  start_min_plan_id:起始切入点在的 路径id 对应的路径 作为开始路径；goal_min_plan_id:切出点在 路径id对应的路径，作为结束路径    , start_min_index 切入点在在行有优先的地图中的索引 ，goal_min_index 切出点在在在行有优先的地图中的索引
        bool find_shortest_pathFig = find_shortest_path( distance_start_map, distance_goal_map, execute_map_plan_Overturn_,all_paths,bfs_result_message);

        if(find_shortest_pathFig == false) {
            cout << "5. 广度优先搜索（BFS）未找到路径 makePlan2Overturn " << endl;
            return false;
        }
        //=========================================================================
        std::cout << "15.makePlan2Overturn 获取半结构全局规划路径,path_id:路径的id,int start_min_index 起始切入路径索引,int goal_min_index:目标切切出点; Point &start_point 起点点位,Point &goal_point终点点位）" <<std::endl;



//        PointDistance start_min_pointDistance ;//todo 找到 起始切入点
//        PointDistance goal_min_pointDistance ;//todo  找到 目标切切出点

        //todo ***********获取半结构全局规划路径,path_id:路径的id,int start_min_index 起始切入路径索引,int goal_min_index:目标切切出点; Point &start_point 起点点位,Point &goal_point终点点位
        // start_min_index ，goal_min_index每个点在行有优先的地图中的索引  //        //获取半结构全局规划路径,path_id:路径的id,, PointDistance &start_min_pointDistance ： 起始切入点,PointDistance &goal_min_pointDistance：目标切切出点; Point &startmone_point 起点点位,Point &goalmone_point终点点位，//找到起始点的最近切入点-，找到 start_x start_y最近的路径对应的点位 Point start_min_point  ；
        //   bool getShortestPathfig =getShortestPath(all_paths,execute_map_plan_,start_min_pointDistance,goal_min_pointDistance,startmone_point,goalmone_point,plan);
        string shortestPath_result_message;//获取半结构全局规划路径 结果信息
        bool getShortestPathfig =getShortestPath(all_paths,execute_map_plan_Overturn_, distance_start_map,distance_goal_map,startmone_point,goalmone_point,plan,shortestPath_result_message,current_path_goTo_target_path_map_plan_Overturn_);

        if(getShortestPathfig == false){
            cout<<"16. error 获取半结构全局规划路径 失败！ getShortestPath"<<endl;
            return false;
        }

        std::cout << "17.makePlan2 add orientations if needed 如果需要，添加方向）" <<std::endl;

//----------------------------------------------------------------------

        // add orientations if needed 如果需要，添加方向
        //   orientation_filter_->processPath(start, plan);//std::vector<geometry_msgs::PoseStamped>& plan
        std::cout << "17.makePlan2 出于可视化目的发布计划" <<std::endl;
        for (int i = 0; i < plan.size(); ++i) {//加方向
            if(i < plan.size() - 1)
                calculate_direction(plan[i+1], plan[i]);
        }



        //publish the plan for visualization purposes 出于可视化目的发布计划
        nav_msgs::Path gui_path;
        publishPlan(plan,gui_path);
        pathPlan_semiStructured_pub_.publish(gui_path);
        std::cout << "18.makePlan2 end 出于可视化目的发布计划" <<std::endl;

        return !plan.empty();//makePlan2
    }





    //添加方向
      void global_planner::calculate_direction(geometry_msgs::PoseStamped const& pose1,
                                           geometry_msgs::PoseStamped& pose2) {
        auto theta = atan2(pose1.pose.position.y - pose2.pose.position.y,
                           pose1.pose.position.x - pose2.pose.position.x);
        pose2.pose.orientation.w = cos(theta / 2);
        pose2.pose.orientation.z = sin(theta / 2);
    }

//============================================================================================================================
//    //切入点起点
//    map<int, PointDistance> distance_start_map;//储存起点到各路径所有点的曼哈顿距离的容器
//    //切出点终点
//    map<int, PointDistance> distance_goal_map;//储存终点到各路径所有点的曼哈顿距离的容器

    // 可以使用广度优先搜索（BFS）算法来找出起始点到目标点的所有路径，并计算每条路径的长度，然后选择路径长度最短的路径作为结果输出。distance_start_map 切入点起点，distance_goal_map 切出点终点， ||std::map<int, MorPlanToMap> map_plan 这里的这个参数不要用引用或指针，会脏数据导致map_plan多一条数据
    bool global_planner::find_shortest_path(map<int, PointDistance> distance_start_map, map<int, PointDistance> distance_goal_map, std::map<int, MorPlanToMap> map_plan,std::vector<std::vector<int>> &all_paths,string &result_message) {

        //todo 打印数据
        std::cout << "0.广度优先搜索（BFS）  map_plan.size() " <<map_plan.size()<<std::endl;
        map<int, MorPlanToMap>::iterator iter;
        for(iter = map_plan.begin(); iter != map_plan.end(); iter++) {
            int current_plan_id = iter->first; //todo 当前路径id，唯一id ；每个路径在数据库中都会存储一个唯一的id
            std::cout << "0.1 广度优先搜索（BFS）  execute_map_plan_ : plan_id: " <<current_plan_id<<std::endl;
        }
        //=========================================**********************************====================================================================

        //todo ================================起点 终点 在这里把数据都找出来=======================================
        unsigned char* costs =origin_costmap_->getCharMap();//获取地图数据
        vector< map<int, int> > start_and_goal_plan_id_map_vector;//保存起始切入点与结束切出点的路径id,的组合。用于广度优先搜索 ；把起点和终点的路径id存储起来，用于后面的路径规划，因为起点和终点的路径id可能不一样；一对一组合，存在多个起点和终点的路径id
       if(isthresholdClosestPoint_){//true:选择最近点作为切入和切出点（切入点与切出点相对固定，取决于起点和终点的位置【满足 切入点最小距离阀值，切出点最小距离阀值的范围内选择】），

           cout<<"1111找到起始点与结束点的最近切入点 （找 小车起点离切入路径最近的一个点路径id，小车终点里路径最近的切出点路径id） "<<endl;
           //todo 1.start---------------------找到起始点与结束点的最近切入点 （找 小车起点离切入路径最近的一个点路径id，小车终点里路径最近的切出点路径id） ----------------------------
           double start_min_distance = POT_HIGH;//将其全部赋值为一个较大的值POT_HIGH
           double goal_min_distance = POT_HIGH;//将其全部赋值为一个较大的值POT_HIGH
           PointDistance start_min_pointDistance ;//todo 找到 起始切入点 ，起始切入点里起点最近的路径点
           PointDistance goal_min_pointDistance ;//todo  找到 目标切切出点，目标切切出点里终点最近的路径点

           double up_start_min_distance = POT_HIGH;//记录上一次的最小直
           PointDistance up_start_min_pointDistance ;//todo 记录上一次起始切入点 ，起始切入点里起点最近的路径点

           //double up_goal_min_distance = POT_HIGH;//记录上一次的最小直
           //PointDistance up_goal_min_pointDistance ;//todo  记录上一次的找到 目标切切出点，目标切切出点里终点最近的路径点

           // start---------------------找到起始切入点------每条路径的一个最小距离点位，找到路径id----------------------
           map<int, PointDistance>::iterator iter6;
           for(iter6 = distance_start_map.begin(); iter6 != distance_start_map.end(); iter6++) {
               //  for (int i = 0; i < distance_vector.size(); ++i) {
               int distance_start_plan_id = iter6->first; //todo 起始切入点，唯一id ；每个路径在数据库中都会存储一个唯一的id
               PointDistance pointDistance_start = iter6->second;//[储存所有到路径点的所有距离数据distance_vector]
               int index = pointDistance_start.index;//点在路径中的索引， 每个点在行有优先的地图中的索引，pointDistance.plan_id 路径中的 点位索引
               int plan_id = pointDistance_start.plan_id;//当前路径的唯一id
               string plan_name = pointDistance_start.plan_name;//当前路径name
               string ns = pointDistance_start.ns;//当前路径 namespace

               //这里要判断在不在障碍物里面 //costmap_->getCharMap() = costs；   lethal_cost_ = 253 致命代价值
               //判定起始点是否在障碍物里面 costmap_->getCharMap() = costs；   lethal_cost_ = 253 致命代价值
               if (costs[index] >= lethal_cost_ && !(unknown_ && costs[index] ==costmap_2d::NO_INFORMATION))//NO_INFORMATION = 255 未知 保留给没有足够信息的单元格。；如果该点的cost值超过lethal_cost_，或者未知但unknown_标志为false，也直接返回
               {
                   cout<<"0.未找到起始点最近切入点  判断在障碍物里面 costs: "<< to_string(costs[index] )<<endl;
                   result_message="未找到起始点最近切入点  判断在障碍物里面";
                   continue;
               }

               if(pointDistance_start.start_distance>=(start_distance_threshold_/origin_costmap_->getResolution())){
                   cout<<"1. 未找到起始点 超过距离阀值 start_distance_threshold ："<<to_string(start_distance_threshold_)<<endl;
                   result_message="未找到起始点 超过距离阀值 start_distance_threshold";
                   continue;
               }
               //起始切入点里起点最近的路径
               if (pointDistance_start.start_distance < start_min_distance) {
                   up_start_min_distance = start_min_distance;//记录上一次的最小直
                   up_start_min_pointDistance =start_min_pointDistance;//todo 记录上一次起始切入点 ，起始切入点里起点最近的路径点
                   //赋值
                   start_min_distance = pointDistance_start.start_distance;
                   start_min_pointDistance = pointDistance_start;

               }



           }//-end--------------------起始点的最近切入点----------------------------

           //-start--------------------结束点的最近切出点----------------------------
           map<int, PointDistance>::iterator iter7;
           for(iter7 = distance_goal_map.begin(); iter7 != distance_goal_map.end(); iter7++) {
               int distance_goal_plan_id=iter7->first; //todo 结束点的最近切出点，唯一id ；每个路径在数据库中都会存储一个唯一的id
               PointDistance pointDistance_goal = iter7->second;//[储存所有到路径点的所有距离数据distance_vector]
               int index = pointDistance_goal.index;//每个点在路径中的索引
               int plan_id = pointDistance_goal.plan_id;//当前路径的唯一id
               string plan_name = pointDistance_goal.plan_name;//当前路径name
               string ns = pointDistance_goal.ns;//当前路径 namespace
               //这里要判断在不在障碍物里面 //costmap_->getCharMap() = costs；   lethal_cost_ = 253 致命代价值
               //这里要判断在不在障碍物里面 //costmap_->getCharMap() = costs；   lethal_cost_ = 253 致命代价值
               //costmap_->getCharMap() = costs；   lethal_cost_ = 253 致命代价值
               if (costs[index] >= lethal_cost_ && !(unknown_ && costs[index] ==costmap_2d::NO_INFORMATION))//NO_INFORMATION = 255 未知 保留给没有足够信息的单元格。；如果该点的cost值超过lethal_cost_，或者未知但unknown_标志为false，也直接返回
               {
                   cout<<"0.BBBBBBBBBBBBB未找到结束点的最近切入点  判断在障碍物里面 "<<endl;
                   result_message="未找到结束点的最近切入点  判断在障碍物里面";
                   continue;
               }
               if(pointDistance_goal.goal_distance>=(goal_distance_threshold_/origin_costmap_->getResolution())){
                   cout<<"2. BBBBBBBBBBBBBBBB未找到结束点的最近切入点 超过距离阀值 goal_distance_threshold ："<<to_string(goal_distance_threshold_)<<endl;
                   result_message="未找到结束点的最近切入点 超过距离阀值 goal_distance_threshold";
                   continue;
               }
               //结束切出点里终点最近的路径
               if (pointDistance_goal.goal_distance < goal_min_distance) {

                  //  up_goal_min_distance = goal_min_distance;//记录上一次的最小直
                  //  up_goal_min_pointDistance=goal_min_pointDistance ;//todo  [记录上一次的找到 目标切切出点，目标切切出点里终点最近的路径点]

                   goal_min_distance = pointDistance_goal.goal_distance;
                   goal_min_pointDistance = pointDistance_goal;

               }

           }//-end--------------------结束点的最近切出点----------------------------

           map<int, int> start_and_goal_plan_id_map;//保存起始切入点与结束切出点的路径id,的组合。用于广度优先搜索 ；把起点和终点的路径id存储起来，用于后面的路径规划，因为起点和终点的路径id可能不一样；一对一组合，存在多个起点和终点的路径id
            /**
           if(start_min_pointDistance.plan_id == goal_min_pointDistance.plan_id && start_min_pointDistance.distance_plan_all_point_index>goal_min_pointDistance.distance_plan_all_point_index){
                cout<<"起始点路径与结束点路径在同一条上，并且结束点在起始点前方，需要重新确定起始点路径，绕着走。"<<endl;
                result_message="起始点路径与结束点路径在同一条上，并且结束点在起始点前方，需要重新确定起始点路径，绕着走。";
               //todo 保存小车起点离切入路径最近的一个点路径id，小车终点里路径最近的切出点路径id。用于广度优先搜索
               start_and_goal_plan_id_map[up_start_min_pointDistance.plan_id]=goal_min_pointDistance.plan_id;//得到的结果值   //还是用原来的直 goal_min_pointDistance ，就把起始点 up_start_min_pointDistance 改掉
               start_and_goal_plan_id_map_vector.push_back(start_and_goal_plan_id_map);//todo //保存起始切入点与结束切出点的路径id的组合。用于广度优先搜索
           }else{
               //todo 保存小车起点离切入路径最近的一个点路径id，小车终点里路径最近的切出点路径id。用于广度优先搜索
               start_and_goal_plan_id_map[start_min_pointDistance.plan_id]=goal_min_pointDistance.plan_id;//得到的结果值
               start_and_goal_plan_id_map_vector.push_back(start_and_goal_plan_id_map);//todo //保存起始切入点与结束切出点的路径id的组合。用于广度优先搜索
           }
           */

           //todo 保存小车起点离切入路径最近的一个点路径id，小车终点里路径最近的切出点路径id。用于广度优先搜索
           start_and_goal_plan_id_map[start_min_pointDistance.plan_id]=goal_min_pointDistance.plan_id;//得到的结果值
           start_and_goal_plan_id_map_vector.push_back(start_and_goal_plan_id_map);//todo //保存起始切入点与结束切出点的路径id的组合。用于广度优先搜索

           //todo end---------------------找到起始点与结束点的最近切入点 （找 小车起点离切入路径最近的一个点路径id，小车终点里路径最近的切出点路径id） ----------------------------

       }else{//false：选择组合路径距离最短的点作为切入和切出点（满足  切入点最小距离阀值，切出点最小距离阀值的范围内选择）
           //保存组合路径距离最短的点作为切入和切出点
           //todo 2.start---------------------找到起始点与结束点的最近切入点  保存起始切入点与结束切出点的路径id,的组合。start_and_goal_plan_id_map ----------------------------
           cout<<"2.start---------------------找到起始点与结束点的最近切入点  保存起始切入点与结束切出点的路径id,的组合。start_and_goal_plan_id_map "<<endl;

           // start---------------------找到起始切入点------每条路径的一个最小距离点位，找到路径id----------------------
           map<int, PointDistance>::iterator iter1;
           for(iter1 = distance_start_map.begin(); iter1 != distance_start_map.end(); iter1++) {
               //  for (int i = 0; i < distance_vector.size(); ++i) {
               int distance_start_plan_id=iter1->first; //todo 起始切入点，唯一id ；每个路径在数据库中都会存储一个唯一的id
               PointDistance pointDistance_start = iter1->second;//[储存所有到路径点的所有距离数据distance_vector]
               int index = pointDistance_start.index;//点在路径中的索引， 每个点在行有优先的地图中的索引，pointDistance.plan_id 路径中的 点位索引
               int plan_id = pointDistance_start.plan_id;//当前路径的唯一id
               string plan_name = pointDistance_start.plan_name;;//当前路径name
               string ns = pointDistance_start.ns;;//当前路径 namespace
               cout<<"3.start---------------------找到起始点与结束点的最近切入点  保存起始切入点与结束切出点的路径id,的组合。start_and_goal_plan_id_map "<<endl;

               //这里要判断在不在障碍物里面 //costmap_->getCharMap() = costs；   lethal_cost_ = 253 致命代价值
               //costmap_->getCharMap() = costs；   lethal_cost_ = 253 致命代价值
               if (costs[index] >= lethal_cost_ && !(unknown_ && costs[index] ==costmap_2d::NO_INFORMATION))//NO_INFORMATION = 255 未知 保留给没有足够信息的单元格。；如果该点的cost值超过lethal_cost_，或者未知但unknown_标志为false，也直接返回
               {
                   cout<<"0.AAAAAAAAAAAAAAAAAAAAAAA未找到起始点最近切入点  判断在障碍物里面 costs: "<< to_string(costs[index] )<<endl;
                   cout<<"0.AAAAAAAAAAAAAAAAAAAAAAA未找到起始点最近切入点  判断在障碍物里面 index: "<< to_string(index )<<endl;
                   result_message="未找到起始点最近切入点  判断在障碍物里面";
                   continue;
               }

               if(pointDistance_start.start_distance>=(start_distance_threshold_/origin_costmap_->getResolution())){
                   cout<<"1. AAAAAAAAAAAAAAAAAAAAAAA未找到起始点 超过距离阀值 start_distance_threshold ："<<to_string(start_distance_threshold_)<<endl;
                   cout<<"2. AAAAAAAAAAAAAAAAAAAAAAA未找到起始点 超过距离阀值 start_distance_threshold_/origin_costmap_->getResolution()) ："<<to_string(start_distance_threshold_/origin_costmap_->getResolution())<<endl;
                   result_message="未找到起始点 超过距离阀值 start_distance_threshold";
                   continue;
               }
               //------------------------------------------------------------------------------------------
               //-start--------------------结束点的最近切出点----------------------------
               // goal_x goal_y 最近的路径对应的点位 distance_goal_vector
               map<int, PointDistance>::iterator iter2;
               for(iter2 = distance_goal_map.begin(); iter2 != distance_goal_map.end(); iter2++) {
                   cout<<"4.start---------------------找到起始点与结束点的最近切入点  保存起始切入点与结束切出点的路径id,的组合。start_and_goal_plan_id_map "<<endl;

                   // for (int i = 0; i < distance_goal_vector.size(); ++i) {
                   int distance_goal_plan_id=iter2->first; //todo 结束点的最近切出点，唯一id ；每个路径在数据库中都会存储一个唯一的id
                   PointDistance pointDistance_goal = iter2->second;//[储存所有到路径点的所有距离数据distance_vector]
                   int index = pointDistance_goal.index;//每个点在路径中的索引
                   int plan_id = pointDistance_goal.plan_id;//当前路径的唯一id
                   string plan_name = pointDistance_goal.plan_name;//当前路径name
                   string ns = pointDistance_goal.ns;//当前路径 namespace


                   //这里要判断在不在障碍物里面 //costmap_->getCharMap() = costs；   lethal_cost_ = 253 致命代价值
                   //costmap_->getCharMap() = costs；   lethal_cost_ = 253 致命代价值
                   if (costs[index] >= lethal_cost_ && !(unknown_ && costs[index] ==costmap_2d::NO_INFORMATION))//NO_INFORMATION = 255 未知 保留给没有足够信息的单元格。；如果该点的cost值超过lethal_cost_，或者未知但unknown_标志为false，也直接返回
                   {
                       cout<<"0.AAAAAAAAAAAAAAAAAAAAAAA未找到结束点的最近切入点  判断在障碍物里面 "<<endl;
                       result_message="未找到结束点的最近切入点  判断在障碍物里面";
                       continue;
                   }
                   if(pointDistance_goal.goal_distance>=(goal_distance_threshold_/origin_costmap_->getResolution())){
                       cout<<"2. AAAAAAAAAAAAAAAAAAAAAAA未找到结束点的最近切入点 超过距离阀值 goal_distance_threshold ："<<to_string(goal_distance_threshold_)<<endl;
                       cout<<"2. AAAAAAAAAAAAAAAAAAAAAAA未找到起始点 超过距离阀值 goal_distance_threshold_/origin_costmap_->getResolution()) ："<<to_string(goal_distance_threshold_/origin_costmap_->getResolution())<<endl;

                       result_message="未找到结束点的最近切入点 超过距离阀值 goal_distance_threshold";
                       continue;
                   }

                   cout<<"5.start---------------------找到起始点与结束点的最近切入点  保存起始切入点与结束切出点的路径id,的组合。start_and_goal_plan_id_map "<<endl;


                   //todo start-----------------------------嵌套找距离最近的路径，包括起始点与切入点-------------------------------------------------------------
                   int start_plan_id= pointDistance_start.plan_id;//起始点的路径id
                   int goal_plan_id=  pointDistance_goal.plan_id;//结束点的路径id
                   map<int, int> start_and_goal_plan_id_map;//保存起始切入点与结束切出点的路径id,的组合。用于广度优先搜索 ；把起点和终点的路径id存储起来，用于后面的路径规划，因为起点和终点的路径id可能不一样；一对一组合，存在多个起点和终点的路径id

                   cout<<"6.start---------------------找到起始点与结束点的最近切入点  保存起始切入点与结束切出点的路径id,的组合。start_and_goal_plan_id_map "<<endl;

                   //todo 保存小车起点离切入路径最近的一个点路径id，小车终点里路径最近的切出点路径id。用于广度优先搜索
                   start_and_goal_plan_id_map[start_plan_id]=goal_plan_id;//todo //保存起始切入点与结束切出点的路径id的组合。用于广度优先搜索
                   start_and_goal_plan_id_map_vector.push_back(start_and_goal_plan_id_map);//todo //保存起始切入点与结束切出点的路径id的组合。用于广度优先搜索
                   cout<<" 2AAAAAAAAAAAAAAAAAAAAAAA起始点路径与结束点路径在同一条上，并且结束点在起始点前方，需要重新确定起始点路径，绕着走。pointDistance_start.start_distance :"<<to_string(pointDistance_start.start_distance)<<endl;
                   cout<<" 2AAAAAAAAAAAAAAAAAAAAAAA起始点路径与结束点路径在同一条上，并且结束点在起始点前方，需要重新确定起始点路径，绕着走。pointDistance_goal.goal_distance :"<<to_string(pointDistance_goal.goal_distance)<<endl;
                   cout<<" 3AAAAAAAAAAAAAAAAAAAAAAA起始点路径与结束点路径在同一条上，并且结束点在起始点前方，需要重新确定起始点路径，绕着走。start_plan_id :"<<to_string(start_plan_id)<<endl;
                   cout<<" 3AAAAAAAAAAAAAAAAAAAAAAA起始点路径与结束点路径在同一条上，并且结束点在起始点前方，需要重新确定起始点路径，绕着走。goal_plan_id :"<<to_string(goal_plan_id)<<endl;
                   cout<<"AAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA嵌套找距离最近的路径，包括起始点与切入点 start_plan_id: "<<to_string(start_plan_id)<<"   ;goal_plan_id: "<<to_string(goal_plan_id)<<endl;
                   cout<<"AAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA嵌套找距离最近的路径，包括起始点与切入点 start_and_goal_plan_id_map_vector.size(): "<<to_string(start_and_goal_plan_id_map_vector.size())<<"   ;start_and_goal_plan_id_map_vector.size(): "<<to_string(start_and_goal_plan_id_map_vector.size())<<endl;

                   /**
                  if(start_plan_id == goal_plan_id && pointDistance_start.start_distance>pointDistance_goal.goal_distance){
                      cout<<" AAAAAAAAAAAAAAAAAAAAAAA起始点路径与结束点路径在同一条上，并且结束点在起始点前方，需要重新确定起始点路径，绕着走。pointDistance_start.start_distance :"<<to_string(pointDistance_start.start_distance)<<endl;
                      cout<<" AAAAAAAAAAAAAAAAAAAAAAA起始点路径与结束点路径在同一条上，并且结束点在起始点前方，需要重新确定起始点路径，绕着走。pointDistance_goal.goal_distance :"<<to_string(pointDistance_goal.goal_distance)<<endl;

                      result_message="起始点路径与结束点路径在同一条上，并且结束点在起始点前方，需要重新确定起始点路径，绕着走。";
                      //todo 保存小车起点离切入路径最近的一个点路径id，小车终点里路径最近的切出点路径id。用于广度优先搜索
                      continue;//todo 重新找起始点的最近切入点
                  }else{
                      //todo 保存小车起点离切入路径最近的一个点路径id，小车终点里路径最近的切出点路径id。用于广度优先搜索
                      start_and_goal_plan_id_map[start_plan_id]=goal_plan_id;//todo //保存起始切入点与结束切出点的路径id的组合。用于广度优先搜索
                      start_and_goal_plan_id_map_vector.push_back(start_and_goal_plan_id_map);//todo //保存起始切入点与结束切出点的路径id的组合。用于广度优先搜索
                      cout<<" 2AAAAAAAAAAAAAAAAAAAAAAA起始点路径与结束点路径在同一条上，并且结束点在起始点前方，需要重新确定起始点路径，绕着走。pointDistance_start.start_distance :"<<to_string(pointDistance_start.start_distance)<<endl;
                      cout<<" 2AAAAAAAAAAAAAAAAAAAAAAA起始点路径与结束点路径在同一条上，并且结束点在起始点前方，需要重新确定起始点路径，绕着走。pointDistance_goal.goal_distance :"<<to_string(pointDistance_goal.goal_distance)<<endl;
                      cout<<" 3AAAAAAAAAAAAAAAAAAAAAAA起始点路径与结束点路径在同一条上，并且结束点在起始点前方，需要重新确定起始点路径，绕着走。start_plan_id :"<<to_string(start_plan_id)<<endl;
                      cout<<" 3AAAAAAAAAAAAAAAAAAAAAAA起始点路径与结束点路径在同一条上，并且结束点在起始点前方，需要重新确定起始点路径，绕着走。goal_plan_id :"<<to_string(goal_plan_id)<<endl;
                  }
                  */


                   // end-----------------------------嵌套找距离最近的路径，包括起始点与切入点-------------------------------------------------------------

               }//todo end for (int i = 0; i < distance_goal_vector.size(); ++i)  找到起始切入点-
               // end---------------------找到起始切入点----------------------------

               //------------------------------------------------------------------------------------------
           }//end  for (int i = 0; i < distance_vector.size(); ++i)
           std::cout << "13.找到起始点与结束点的最近切入点 结束点的最近切出点" <<std::endl;
           //todo .end---------------------找到起始点与结束点的最近切入点  保存起始切入点与结束切出点的路径id,的组合。start_and_goal_plan_id_map ----------------------------

       }//end if(isthresholdClosestPoint_)

       //todo .end---------------------------------------------------------------------------------------------------------------------------------------------------
        cout<<"4. 找到起始点与结束点的最近切入点 distance_start_map.size(): "<<to_string(distance_start_map.size())<<"   ;distance_goal_map.size(): "<<to_string(distance_goal_map.size())<<endl;
        cout<<"4. 找到起始点与结束点的最近切入点 distance_start_map.start_and_goal_plan_id_map_vector.size()(): "<<to_string(start_and_goal_plan_id_map_vector.size())<<endl;

        //=========================================**********************************====================================================================
        //=========================================**********************************====================================================================

        //todo 3. start==============================广度优先搜索（BFS）===============================================================================================
        if(start_and_goal_plan_id_map_vector.size()==0 ){
            std::cout << "0.3 广度优先搜索（BFS） 保存起始切入点与结束切出点的路径id,的组合。用于广度优先搜索 start_and_goal_plan_id_map.size()==0" <<std::endl;
            result_message="0.3 广度优先搜索（BFS）保存起始切入点与结束切出点的路径id,的组合。用于广度优先搜索,  start_and_goal_plan_id_map.size()==0";
            return false;
        }


        for (int k = 0; k < start_and_goal_plan_id_map_vector.size(); ++k) {
            map<int, int> start_and_goal_plan_id_map = start_and_goal_plan_id_map_vector[k];
            if( start_and_goal_plan_id_map.size()==0){
                std::cout << "0.3 广度优先搜索（BFS） 保存起始切入点与结束切出点的路径id,的组合。用于广度优先搜索 start_and_goal_plan_id_map.size()==0" <<std::endl;
                result_message="0.3 广度优先搜索（BFS）保存起始切入点与结束切出点的路径id,的组合。用于广度优先搜索,  start_and_goal_plan_id_map.size()==0";
                return false;
            }

            map<int, int>::iterator iter5; //
            for(iter5 = start_and_goal_plan_id_map.begin(); iter5 != start_and_goal_plan_id_map.end(); iter5++) {
                std::cout << "start_and_goal_plan_id_map  key: " << iter5->first << " value: " << iter5->second << std::endl;
                int start_id = iter5->first ;//起始点的路径id
                int goal_id  =  iter5->second;//结束点的路径id

                //todo ==============================广度优先搜索（BFS）===========================================
                std::cout << "0.2 广度优先搜索（BFS）  判断 execute_map_plan 中是否存在键值" <<std::endl;
                //查找键值为 起点路径 id： start_id 的元素，判断 execute_map_plan 中是否存在键值为 start_id 的元素
                auto posKey_start_id=map_plan.find(start_id);
                if(posKey_start_id==map_plan.end()){
                    std::cout<<"/+++++++++++++++///error 0.3 广度优先搜索 当前路径 execute_map_plan 对应的路径不存在 start_id    :"<<std::to_string(start_id)<< std::endl;
                    result_message="/+++++++++++++++///error 0.3 广度优先搜索 当前路径 execute_map_plan 对应的路径不存在 start_id    :"+std::to_string(start_id);
                    continue;
                }

                //查找键值为 终点路径 id： end_id 的元素，判断 execute_map_plan 中是否存在键值为 start_id 的元素        auto posKey_start_id=map_plan.find(end_id);
                auto posKey_end_id=map_plan.find(goal_id);
                if(posKey_end_id==map_plan.end()){
                    std::cout<<"/+++++++++++++++///error 0.4 广度优先搜索 当前路径 execute_map_plan 对应的路径不存在 goal_id    :"<<std::to_string(goal_id)<< std::endl;
                    result_message="/+++++++++++++++///error 0.4 广度优先搜索 当前路径 execute_map_plan 对应的路径不存在 goal_id    :"+std::to_string(goal_id);

                    continue;
                }

                ///-----------------------------------------------------

                std::queue<std::vector<int>> q; // 定义BFS使用的队列，每个元素是一个存储路径的vector
                q.push({start_id}); // 将起始点存储为第一个元素
                std::cout << "0.广度优先搜索（BFS）  start_id " <<start_id<<std::endl;
                // std::vector<std::vector<int>> all_paths; // 存储所有路径; [sukai 提取出来] 存储所有路径的id
                int shortest_path_length = INT_MAX; // 存储路径长度最短的路径长度
                int first_i=0;//如果第一个点的起点和终点就在一条路径上，需要继续找一条路径
                bool first=true;//如果第一个点的起点和终点就在一条路径上，需要继续找一条路径
                while (!q.empty()) {
                    first_i++;
                    std::vector<int> path = q.front(); // 取出队列中的第一个元素（路径）
                    q.pop();
                    int last_node = path.back(); // 取出路径中的最后一个节点
                    std::cout << "1.广度优先搜索（BFS）  start_id " <<start_id<<std::endl;
                    std::cout << "2.广度优先搜索（BFS）  goal_id " <<goal_id<<std::endl;
                    std::cout << "3.广度优先搜索（BFS） last_node  : " <<last_node<<std::endl;
                    std::cout << "4.广度优先搜索（BFS） shortest_path_length  : " <<shortest_path_length<<std::endl;

                    std::cout << "4.广度优先搜索（BFS） first_i  : " <<first_i<<std::endl;

                    if(first_i==1 && start_id==goal_id){//如果第一个点的起点和终点就在一条路径上，需要继续找一条路径
                        PointDistance pointDistance_start =  distance_start_map[last_node];
                        PointDistance pointDistance_goal =   distance_goal_map[goal_id];
                        if(pointDistance_goal.distance_plan_all_point_index<pointDistance_start.distance_plan_all_point_index){//切出点在切入点的前面
                            cout<<"4.1 切出点在切入点的前面 pointDistance_goal.goal_distance: "<<to_string(pointDistance_goal.distance_plan_all_point_index)<<"   ;pointDistance_start.start_distance: "<<to_string(pointDistance_start.distance_plan_all_point_index)<<endl;
                            first= false;
                        }
                    }else{
                        first= true;
                        first_i=3;
                    }



                    // if (last_node == end_id && first！=1) { // 如果最后一个节点是目标节点，则将此路径存储到all_paths中 【 已经找到一条从start到end的路径】
                    if (last_node == goal_id && first) { // 如果最后一个节点是目标节点，则将此路径存储到all_paths中 【 已经找到一条从start到end的路径】

                        all_paths.push_back(path);
                        if (path.size() < shortest_path_length) { // 如果此路径比之前存储的路径更短，则更新最短路径长度
                            shortest_path_length = path.size();
                        }
                        std::cout << "5.广度优先搜索（BFS） shortest_path_length  : " <<shortest_path_length<<std::endl;

//                        if ( first_i==1){//todo  如果第一个点的起点和终点就在一条路径上，需要继续找一条路径
//
//                            std::cout << "5.1.广度优先搜索（BFS） last_node  : " <<last_node<<std::endl;
//                            MorPlanToMap current_plan = map_plan[last_node]; // 获取当前节点能到达的路径
//                            std::cout << "5.2.广度优先搜索（BFS） last_node  : " <<last_node<<std::endl;
//
//                            for (int next_node_id : current_plan.target_plan_ids) { // 遍历当前节点能到达的路径
//                                std::cout << "5.3.广度优先搜索（BFS）遍历当前节点能到达的路径 next_node_id  : " <<next_node_id<<std::endl;
//                                if (std::find(path.begin(), path.end(), next_node_id) == path.end()) { // 如果此路径没有被访问过，则将此路径加入到队列中
//                                    std::vector<int> new_path = path;
//                                    new_path.push_back(next_node_id);
//                                    q.push(new_path);
//                                    std::cout << "5.4.广度优先搜索（BFS） 如果此路径没有被访问过，则将此路径加入到队列中  :next_node_id " <<next_node_id<<std::endl;
//
//                                }
//                            }
//
//                        }//if ( first==true)

                        first_i=3;//恢复成3，确保初始化时是0，第一次进循环后加1，后面就一直默认3。上面只需要运行一次


                    } else {
                        std::cout << "6.广度优先搜索（BFS） last_node  : " <<last_node<<std::endl;

                        MorPlanToMap current_plan = map_plan[last_node]; // 获取当前节点能到达的路径
                        std::cout << "7.广度优先搜索（BFS） last_node  : " <<last_node<<std::endl;
                        std::cout << "7.广度优先搜索（BFS） current_plan.target_plan_ids.size()  : " <<current_plan.target_plan_ids.size()<<std::endl;
                        for (int next_node_id : current_plan.target_plan_ids) { // 遍历当前节点能到达的路径
                            std::cout << "8.广度优先搜索（BFS）遍历当前节点能到达的路径 next_node_id  : " <<next_node_id<<std::endl;
                              //  if ((std::find(path.begin(), path.end(), next_node_id) == path.end())) { //todo path中不存在就加入；  去除已经遍历过的数据；  如果此路径没有被访问过，则将此路径加入到队列中
                                if ((std::find(path.begin(), path.end(), next_node_id) == path.end())||next_node_id==goal_id) { //todo path中不存在就加入；  去除已经遍历过的数据；  如果此路径没有被访问过，则将此路径加入到队列中;或者第一个是终点 存在的  next_node_id==goal_id  就加入
                                std::vector<int> new_path = path;//path.back() 取出路径中的最后一个节点， 剩下的数据重新赋值给 new_path
                                new_path.push_back(next_node_id);//将当前未处理过的节点加入到路径中
                                q.push(new_path);//把path重新加入到队列
                                std::cout << "9.广度优先搜索（BFS） 如果此路径没有被访问过，则将此路径加入到队列中  :next_node_id " <<next_node_id<<std::endl;

                            }
                        }
                    }
                }//todo end ==============================广度优先搜索（BFS）===========================================




            }//end for(iter5 = start_and_goal_plan_id_map.begin(); iter5 != start_and_goal_plan_id_map.end(); iter5++)





        }//todo end for (int k = 0; k < start_and_goal_plan_id_map_vector.size(); ++k)

        //todo     end==============================广度优先搜索（BFS）================================================================================================


        //=========================================**********************************====================================================================
        //=========================================**********************************====================================================================
        std::cout << "10.广度优先搜索（BFS）输出所有路径 all_paths.size():" <<all_paths.size()<<std::endl;
        //todo 如果没有找到路径，则返回false
        if(all_paths.size() == 0){
            std::cout << "error 没有找到联通的路径 11.广度优先搜索（BFS）输出所有路径 all_paths.size():" <<all_paths.size()<<std::endl;
            result_message="广度优先搜索 没有找到能联通的路径，请检查是否存在到达目标路径的上一条路径  all_paths.size() :"+std::to_string(all_paths.size());
            return false;
        }
        // 输出所有路径
        std::cout << "start***********************************输出所有路径 all_paths path: "<<std::endl;

        for (std::vector<int> path : all_paths) {
            std::cout << "Path: ";
            for (int node_id : path) {
                std::cout << node_id << "->";
            }
            std::cout << "End\n";
        }
        std::cout << "end***********************************输出所有路径 all_paths path: ";

        // 输出最短路径
        std::cout << "start ***********************************输出最短路径 Shortest path: ";
        for (int node_id : all_paths[0]) {
            std::cout << node_id << "->";
        }
        std::cout << "End\n";


        std::cout << "End 输出最短路径" <<std::endl;

        //todo 打印
        map<int, MorPlanToMap>::iterator iter3;
        for(iter3 = map_plan.begin(); iter3 != map_plan.end(); iter3++) {
            int current_plan_id = iter3->first; //todo 当前路径id，唯一id ；每个路径在数据库中都会存储一个唯一的id
            std::cout << "0.00000 打印  execute_map_plan_ : plan_id: " <<current_plan_id<<std::endl;
        }


        cout<<"XXXXXXX 找到起始点与结束点的最近切入点 distance_start_map.size(): "<<to_string(distance_start_map.size())<<"   ;distance_goal_map.size(): "<<to_string(distance_goal_map.size())<<endl;
        cout<<"XXXXX找到起始点与结束点的最近切入点 distance_start_map.start_and_goal_plan_id_map_vector.size()(): "<<to_string(start_and_goal_plan_id_map_vector.size())<<endl;

        return true;
    }


    //=====================================================================

    //获取半结构全局规划路径,path_id:路径的id, PointDistance &start_min_pointDistance ： 起始切入点,PointDistance &goal_min_pointDistance：目标切切出点; Point &start_point 起点点位,Point &goal_point终点点位
   // bool global_planner::getShortestPath(std::vector<std::vector<int>> &all_paths,map<int, MorPlanToMap> &execute_map_plan,PointDistance &start_min_pointDistance,PointDistance &goal_min_pointDistance,Point &start_point,Point &goal_point,std::vector<geometry_msgs::PoseStamped> &plan){
        bool global_planner::getShortestPath(std::vector<std::vector<int>> &all_paths,map<int, MorPlanToMap> &execute_map_plan, map<int, PointDistance> distance_start_map,map<int, PointDistance> distance_goal_map,Point &start_point,Point &goal_point,std::vector<geometry_msgs::PoseStamped> &plan,string &result_message,map<int, map<int, CurrentPathGoToTargetPath>> current_path_goTo_target_path_map_plan){

        //==============================================================================================

        //切入点起点
       // map<int, PointDistance> distance_start_map;//储存起点到各路径所有点的曼哈顿距离的容器
        //切出点终点
       // map<int, PointDistance> distance_goal_map;//储存终点到各路径所有点的曼哈顿距离的容器

        int cost_min =POT_HIGH;//当前路径的cost
        int all_paths_vector_min_index =-1;//todo 当前最小cost路径 在all_paths vector 集合的位置，// std::vector<int> shortest_path_ids=all_paths[index_min]; 获取const代价最小的路径，再获取通过这个最短路径std::vector<int> shortest_path_ids集合
        Point start_min_point;//切入点像素坐标
        Point goal_min_point;//切出点像素坐标
        PointDistance pointDistance_start_min;//起始切入点
        PointDistance pointDistance_goal_min;//目标切出点

        //todo start-----------------------------嵌套找距离最近的路径，包括起始点与切入点-------------------------------------------------------------
        //todo 1.把路径与路径联通不了的路径去掉
        for (int j = 0; j < all_paths.size(); ++j) {
            std::vector<int> path_ids=all_paths[j];
            //======================================
            //todo 1.把路径与路径联通不了的路径去掉
            std::cout<<"1.+++++++++++++++把路径与路径联通不了的路径去掉  all_paths.size()  :"<<std::to_string(all_paths.size())<< std::endl;

            for (int z = 0;  z< path_ids.size(); z++) {
                int path_id= path_ids[z];
                //查找键值为k的元素，判断map中是否存在键值为k的元素
                auto posKey=current_path_goTo_target_path_map_plan.find(path_id);
                if(posKey!=current_path_goTo_target_path_map_plan.end()){
                    if(z>=(path_ids.size()-1)){
                        cout<<"0.查找键值为k的元素，判断map中是否存在键值为k的元素 path_id:"<<path_id<<endl;
                        cout<<"0.1查找键值为k的元素，判断map中是否存在键值为k的元素 path_id:"<<path_id<<endl;
                        break;
                    }


                    cout<<"1.1把路径与路径联通不了的路径去掉 posKey->first:"<<posKey->first<<endl;
                    cout<<"1.2 把路径与路径联通不了的路径去掉 path_id:"<<path_id<<endl;
                    if((z+1)<(path_ids.size()-1))
                         cout<<"1.3 把路径与路径联通不了的路径去掉 path_ids[z+1]:"<<path_ids[z+1]<<endl;
                    map<int, CurrentPathGoToTargetPath> target_map_plan =   current_path_goTo_target_path_map_plan[path_ids[z]];
                    //打印
                    map<int, CurrentPathGoToTargetPath>::iterator iter6;
                    //通过每条路径的索引得到像素坐标
                    //todo 1.从半结构化每条路径中的所有点的中找最小曼哈顿距离的点位置【每条路径都对应一个可到达的（距曼哈顿离最短）点位】 [储存所有到路径点的起点距离数据 distance_vector] [储存所有到路径点的终点距离数据 distance_goal_vector]
                    for(iter6 = target_map_plan.begin(); iter6 != target_map_plan.end(); iter6++) {
                        std::cout<<"2.3.+++++++++++++++把路径与路径联通不了的路径去掉  iter->first :"<<iter6->first<< std::endl;
                    }


                    auto posKey1=target_map_plan.find(path_ids[z+1]);
                    if(posKey1==target_map_plan.end()){
                        std::cout<<"/+++++++++++++++///error 把路径与路径联通不了的路径 all_paths 中 去掉 current_path_goTo_target_path_map_plan path_ids[z+1] :"<<std::to_string(path_ids[z+1])<< std::endl;
                        //todo 1.把路径与路径联通不了的路径去掉
                        all_paths.erase(all_paths.begin()+j);
                        j--;
                        std::cout<<"3.+++++++++++++++把路径与路径联通不了的路径去掉  all_paths.size()  :"<<std::to_string(all_paths.size())<< std::endl;

                        break;
                    }
                }else{
                    std::cout<<"/+++++++++++++++///把路径与路径联通不了的路径 all_paths 中 去掉 current_path_goTo_target_path_map_plan path_id   :"<<std::to_string(path_id)<< std::endl;
                    //todo 1.把路径与路径联通不了的路径去掉
                    all_paths.erase(all_paths.begin()+j);
                    j--;
                    std::cout<<"2.+++++++++++++++把路径与路径联通不了的路径去掉  all_paths.size()  :"<<std::to_string(all_paths.size())<< std::endl;

                    break;

                }//if(posKey!=current_path_goTo_target_path_map_plan.end())

                std::cout<<"4.+++++++++++++++把路径与路径联通不了的路径去掉  all_paths.size()  :"<<std::to_string(all_paths.size())<< std::endl;

            }
        }//for (int j = 0; j < all_paths.size(); ++j)
        //todo end.把路径与路径联通不了的路径去掉

        for (int j = 0; j < all_paths.size(); ++j) {
            std::vector<int> path_ids=all_paths[j];

            int start_plan_id=  -1;//切入点路径
            int goal_plan_id=   -1;//切出点路径
            if(path_ids.size()==0){
                continue;
            } else if(path_ids.size()==1){//切入点路径与切出点路径再同一条路径，只有一条路径
                 start_plan_id=  path_ids[0];//切入点路径
                 goal_plan_id=  path_ids[0];//切出点路径
                std::cout << "切入点路径与切出点路径再同一条路径，只有一条路径 path_ids.size():" << path_ids.size() << std::endl;
            }else {
                start_plan_id=   path_ids[0];//切入点路径
                goal_plan_id =   path_ids[path_ids.size()-1];//切出点路径
                std::cout << "path_ids.size():" << path_ids.size() << std::endl;
            }
            if(start_plan_id==-1||goal_plan_id==-1){
                std::cout<<"/+++++++++++++++///error 6: path_ids  对应的路径不存在  start_plan_id   :"<<std::to_string(start_plan_id)<< std::endl;
                continue;
            }

            auto posKey=distance_start_map.find(start_plan_id);
            if(posKey==distance_start_map.end()){
                std::cout<<"/+++++++++++++++///error 6:distance_start_map对应的路径不存在  start_plan_id   :"<<std::to_string(start_plan_id)<< std::endl;
                continue;
            }

            auto posKey1=distance_goal_map.find(goal_plan_id);
            if(posKey1==distance_goal_map.end()){
                std::cout<<"/+++++++++++++++///error 7:distance_goal_map对应的路径不存在  start_plan_id   :"<<std::to_string(goal_plan_id)<< std::endl;
                continue;
            }

            //todo 1.切入点
            PointDistance pointDistance_start = distance_start_map[start_plan_id];//[储存所有到路径点的所有距离数据distance_vector]
            int plan_id = pointDistance_start.plan_id;//当前路径的唯一id
            int start_result_index=pointDistance_start.distance_plan_all_point_index;//返回的在vector列表de索引
            int start_min_index=pointDistance_start.index; //切入点在行有优先的地图中的索引
            double start_distance = pointDistance_start.start_distance;////计算起点到当前路径的曼哈顿距离
            //todo 1.切出点
            PointDistance pointDistance_goal =  distance_goal_map[goal_plan_id];//[储存所有到路径点的所有距离数据distance_vector]
            int goal_result_index =pointDistance_goal.distance_plan_all_point_index;//返回的在vector列表de索引
            int goal_min_index=pointDistance_goal.index;   //切出点在行有优先的地图中的索引
            double goal_distance  = pointDistance_goal.goal_distance;///计算终点到当前路径的曼哈顿距离

             //cost 数据
            int costcount =(int)start_distance+(int)goal_distance;//当前路径的cost 起点到当前路径的曼哈顿距离
            std::cout<<"costcount****************起始切入点与结束切出点在同一路径上并且，切出路径在切入路径的前方  ********************costcount   :"<<std::to_string(costcount)<< std::endl;

            //把每条路径的距离求出来作为cost直,再累加起来作为总cost
            for (int i = 0; i < path_ids.size(); ++i) {
                int path_id= path_ids[i];
                //todo
                std::cout <<"path_id: "<< path_id << std::endl;

                MorPlanToMap  current_morPlanToMap=   execute_map_plan[path_id];
                std::vector<int> plan_indexs = current_morPlanToMap.plan_indexs;//当前路径在地图中每个点的索引
                vector<Point>  distance_plan_all_vector =  current_morPlanToMap.distance_plan_all_vector;////todo 保存路径 所有点的像素坐标

                int cost =-1;//当前路径的cost
                //判断起始切入点是否在当前的路径上,并获得列表对应的id
//                int start_result_index;//返回的在vector列表de索引
//                bool start_index_is_index_in_planfig =is_index_in_planFun(start_min_index,plan_indexs,start_result_index);
//
//                //判断终点切出点是否在当前的路径上,并获得列表对应的id
//                int goal_result_index;//返回的在vector列表de索引
//                bool goal_index_is_index_in_planfig =is_index_in_planFun(goal_min_index,plan_indexs,goal_result_index);

                if(path_ids.size()==1){//只有一条路径,起始点和结束点都是当前路径的起始点和结束点
//                    if(!start_index_is_index_in_planfig||!goal_index_is_index_in_planfig){
//                        std::cout<<"/+++++++++++++++///error 1:判断起始切入点是否在当前的路径上,判断终点切出点是否在当前的路径上 不在结构化网格上     :"<<std::to_string(start_min_index)<< std::endl;
//                        return false;
//                    }
                    cost = goal_result_index-start_result_index;
                    if(cost<=-1){
                        std::cout<<"****************起始切入点与结束切出点在同一路径上并且，切出路径在切入路径的前方  ********************   :"<<std::to_string(goal_min_index)<< std::endl;
                        costcount=POT_HIGH;//赋值无穷大，重新找一条路径，（goal_result_index-start_result_index）这条路径不能倒着行
                        continue;
                    }else{
                        costcount+=cost;
                    }


                }else{
                    if(i==0){//起始点
//                        if(!start_index_is_index_in_planfig){
//                            std::cout<<"/+++++++++++++++///error 2:判断起始切入点是否在当前的路径上,不在结构化网格上     :"<<std::to_string(start_min_index)<< std::endl;
//
//                            return false;
//                        }
                        //计算代价
                        cost = distance_plan_all_vector.size()-start_result_index;//cost start_result_index->distance_plan_all_vector.size()
                        costcount+=cost;
                    }else if(i==path_ids.size()-1){//结束点

//                        if(!goal_index_is_index_in_planfig){
//                            std::cout<<"/+++++++++++++++///error 3:判断终点切出点是否在当前的路径上 不在结构化网格上     :"<<std::to_string(goal_min_index)<< std::endl;
//                            return false;
//                        }
                        cost   = goal_result_index;//cost 0->goal_result_index
                        costcount+=cost;


                    }else{//中间点
                        costcount+=distance_plan_all_vector.size();

                    }
                }//-----------------结束if语句-----------------

               }//2.结束for循环

            std::cout << "End\n";
            if(cost_min>costcount){//如果当前路径的cost小于之前的cost,则更新cost
                cost_min =costcount;
                all_paths_vector_min_index=j; //todo 当前最小cost路径 在all_paths vector 集合的位置，// std::vector<int> shortest_path_ids=all_paths[all_paths_vector_min_index]; 获取const代价最小的路径，再获取通过这个最短路径std::vector<int> shortest_path_ids集合
                start_min_point=pointDistance_start.point;//切入点像素坐标
                goal_min_point=pointDistance_goal.point;//切出点像素坐标
                pointDistance_start_min=pointDistance_start;//起始切入点
                pointDistance_goal_min=pointDistance_goal;//目标切出点
                std::cout<<" 3.5:找到最小的cost路径 当前路径的最小cost对应的路径 :all_paths_vector_min_index "<<std::to_string(all_paths_vector_min_index)<<" :cost_min:  "<<std::to_string(cost_min)<< std::endl;
            }
        }//1.结束for循环  all_paths.size()


        //=============================================================================================


        //start_min_index ，goal_min_index每个点在行有优先的地图中的索引


        std::cout<<" 2:获取半结构全局规划路径   起始点 坐标 start_point.x :"<<std::to_string(start_point.x)<<" :start_point.y:  "<<std::to_string(start_point.y)<< std::endl;
        std::cout<<" 2:获取半结构全局规划路径   结束点 坐标 goal_point.x :" <<std::to_string(goal_point.x)<<"  : goal_point.y:  "<<std::to_string(goal_point.y)<< std::endl;

        std::cout<<" 3:获取半结构全局规划路径   最近切入点  坐标 start_min_point.x :"<<std::to_string(start_min_point.x)<<" : start_min_point.y:  "<<std::to_string(start_min_point.y)<< std::endl;
        std::cout<<" 3:获取半结构全局规划路径   最近切出点  坐标 goal_min_point.x :" <<std::to_string(goal_min_point.x)<<"  : goal_min_point.y:  "<<std::to_string(goal_min_point.y)<< std::endl;

        std::cout<<" 3:当前路径的最小cost对应的路径 " <<std::endl;

        std::cout<<" 4:找到最小的cost路径 当前路径的最小cost对应的路径 :all_paths_vector_min_index "<<std::to_string(all_paths_vector_min_index)<<" :cost_min:  "<<std::to_string(cost_min)<< std::endl;

        //=====================================================================================================================
        if(all_paths_vector_min_index==-1){//todo 当前最小cost路径 在all_paths vector 集合的位置，// std::vector<int> shortest_path_ids=all_paths[index_min]; 获取const代价最小的路径 id，再获取通过这个最短路径std::vector<int> shortest_path_ids集合

            std::cout<<"/+++++++++++++++///error 4:没有找到最小的cost路径  all_paths_vector_min_index   :"<<std::to_string(all_paths_vector_min_index)<< std::endl;
            result_message="没有找到最小的cost路径 all_paths_vector_min_index   :"+std::to_string(all_paths_vector_min_index);
            return false;
        }

        //定义一个总的路径
        // std::vector<geometry_msgs::PoseStamped> plan;
        //todo 1.获取const代价最小的路径
        std::vector<int> shortest_path_ids=all_paths[all_paths_vector_min_index];
        int start_result_index= pointDistance_start_min.distance_plan_all_point_index;
        int goal_result_index= pointDistance_goal_min.distance_plan_all_point_index;
        int  start_min_index=pointDistance_start_min.index;////切入点在行有优先的地图中的索引
        int  goal_min_index=pointDistance_goal_min.index;   //切出点在行有优先的地图中的索引

        if(shortest_path_ids.size()==0){
            std::cout<<"/+++++++++++++++///error 4:没有找到最小的cost路径  shortest_path_ids.size()   :"<<std::to_string(shortest_path_ids.size())<< std::endl;
            result_message="没有找到最小的cost路径 shortest_path_ids.size()   :"+std::to_string(shortest_path_ids.size());
            return false;
        }

        //=======================================================
        //todo 答打印调试
        for (int k = 0; k < shortest_path_ids.size(); ++k) {
            int path_id = shortest_path_ids[k];
            std::cout<<" ****4:找到最小的cost路径 当前路径的最小cost对应的路径1 :all_paths[all_paths_vector_min_index].size() "<<std::to_string(shortest_path_ids.size())<< std::endl;
            std::cout<<" *** 4:找到最小的cost路径 当前路径的最小cost对应的路径1 :path_id "<<std::to_string(path_id)<< std::endl;
            std::cout<<" *** 4:找到最小的cost路径 当前路径的最小cost对应的路径1 :cost_min "<<std::to_string(cost_min)<< std::endl;
            std::cout<<" *** 4:找到最小的cost路径 当前路径的最小cost对应的路径1 :all_paths_vector_min_index "<<std::to_string(all_paths_vector_min_index)<< std::endl;
            std::cout<<" *** 4:找到最小的cost路径 当前路径的最小cost对应的路径1 :k "<<std::to_string(k)<< std::endl;

        }

        //======================================================
        //todo 2.起始点到切入点的路径
        std::vector<geometry_msgs::PoseStamped> startplan;
        std::vector<Point> startplan_points;
        std::cout<<" 5: makePlanWithSemiStructuredPath2 从起始点规划一条切入点路径 "<< std::endl;  //Point &start_min_point,Point &goal_min_point
        // algorithm= Dijkstra 算法 ; AStar 算法   start_point：起始点 ，start_min_point：切入点
        bool  startpathfig=makePlanWithSemiStructuredPath2(start_point,start_min_point, startplan,startplan_points,"AStar");
        if(!startpathfig){
            std::cout<<"/+++++++++++++++///error 5:起始点到切入点的路径规划失败     :"<<std::to_string(all_paths_vector_min_index)<< std::endl;
            return false;
        }

        std::cout<<" 4.9 :组成完整的路径 plan.size(): "<< plan.size()<< std::endl;

        //todo 1.组成完整的路径 (加入起始点)
        plan.insert(plan.end(), startplan.begin(), startplan.end());




        std::cout<<" 5:组成完整的路径 plan.size(): "<< plan.size()<< std::endl;

        //组成完整的路径
        for (int k = 0; k < shortest_path_ids.size(); ++k) {
            int path_id = shortest_path_ids[k];
            std::cout<<"path_id    :"<<std::to_string(path_id)<< std::endl;

            //查找键值为path_id的元素，判断map中是否存在键值为path_id的元素
            auto posKey=execute_map_plan.find(path_id);
            if(posKey==execute_map_plan.end()){
                std::cout<<"/+++++++++++++++///error 6:当前路径的最小cost对应的路径不存在     :"<<std::to_string(pointDistance_start_min.index)<< std::endl;
                return false;
            }

            //todo 获取当前路径的信息
            MorPlanToMap  current_morPlanToMap=   execute_map_plan[path_id];

            std::vector<int> plan_indexs = current_morPlanToMap.plan_indexs;//当前路径在地图中每个点的索引
            vector<Point>  distance_plan_all_vector =  current_morPlanToMap.distance_plan_all_vector;////todo 保存路径 所有点的像素坐标
            std::vector<geometry_msgs::PoseStamped> current_plan = current_morPlanToMap.current_plan;//todo 当前路径




             /**
            //判断起始切入点是否在当前的路径上,并获得列表对应的id
            int start_result_index;//返回的在vector列表de索引
            bool start_index_is_index_in_planfig =is_index_in_planFun(start_min_index,plan_indexs,start_result_index);

            //判断终点切出点是否在当前的路径上,并获得列表对应的id
            int goal_result_index;//返回的在vector列表de索引
            bool goal_index_is_index_in_planfig =is_index_in_planFun(goal_min_index,plan_indexs,goal_result_index);
            std::cout<<" 6:只有一条路径,起始点和结束点都是当前路径的起始点和结束点"<< std::endl;
            std::cout<<" 6.1:判断起始切入点是否在当前的路径上,并获得列表对应的id，返回的在std::vector<int> plan_indexs 列表de索引  start_result_index :" <<std::to_string(start_result_index)<<"  : goal_result_index:  "<<std::to_string(goal_result_index)<< std::endl;
            std::cout<<" 6.2:判断起始切入点是否在当前的路径上   current_plan.size() :" <<std::to_string(current_plan.size())<< std::endl;
            std::cout<<" 6.3 判断起始切入点是否在当前的路径上 切入点或切出点超出所在 路径的范围   distance_plan_all_vector.size() :" <<std::to_string(distance_plan_all_vector.size())<< std::endl;

            if(start_result_index>current_plan.size()||goal_result_index>current_plan.size()){
                std::cout<<" 6.3 /+++++++++++++++///error 超出范围 并获得列表对应的id，返回的在std::vector<int> plan_indexs 列表de索引  start_result_index :" <<std::to_string(start_result_index)<<"  : goal_result_index:  "<<std::to_string(goal_result_index)<< std::endl;
                std::cout<<" 6.4 /+++++++++++++++///error 超出范围 切入点或切出点超出所在 路径的范围   current_plan.size() :" <<std::to_string(current_plan.size())<< std::endl;
                std::cout<<" 6.5 /+++++++++++++++///error 超出范围 切入点或切出点超出所在 路径的范围   distance_plan_all_vector.size() :" <<std::to_string(distance_plan_all_vector.size())<< std::endl;
                return false;
            }

            */

            if(shortest_path_ids.size()==1){//只有一条路径,起始点和结束点都是当前路径的起始点和结束点

//                if(!start_index_is_index_in_planfig||!goal_index_is_index_in_planfig){
//                    std::cout<<"/+++++++++++++++///error 1:判断起始切入点是否在当前的路径上,判断终点切出点是否在当前的路径上 不在结构化网格上     :"<<std::to_string(start_min_index)<< std::endl;
//                    return false;
//                }
//              //todo
                if(start_result_index>goal_result_index) {
                    std::cout << "/+++++++++++++++/// 1: 起始点 在 结束点 的后面，小车可倒着走 ；只有一条路径,起始点和结束点都是当前路径的起始点和结束点  start_result_index:"<< std::to_string(start_result_index) << std::endl;
                    std::cout << "/+++++++++++++++/// 1: 起始点 在 结束点 的后面，小车可倒着走 ；只有一条路径,起始点和结束点都是当前路径的起始点和结束点  goal_result_index:"<< std::to_string(goal_result_index) << std::endl;

                    //判断能不能倒着走,是否单向导航 true 单向导航，false 双向导航
                    if(!isOneWay_){//翻转导航
                        try {

                            //获取状态机获取
                            geometry_msgs::PoseStamped start_pose, target_pose, target_pose_default;
                            std::vector<geometry_msgs::PoseStamped> plan_bendi;
                            std::string current_string;
                            boost::any age0,age1,age2,age3;
                            state_machine_.get_current_state_param(0,age0);//起始点
                            state_machine_.get_current_state_param(1,age1);//目标点
                            state_machine_.get_current_state_param(2,age2);//默认点
                            state_machine_.get_current_state_param(3,age3);//获取当前状态的参数

                            start_pose = any_cast<geometry_msgs::PoseStamped>(age0);
                            target_pose = any_cast<geometry_msgs::PoseStamped>(age1);
                            target_pose_default = any_cast<geometry_msgs::PoseStamped>(age2);
                            current_string = any_cast<std::string>(age3);

                            // 获取当前状态的成员函数
                            std::string  current_state = state_machine_.get_current_state();
                            ROS_INFO_STREAM("2.创建Twist消息并设置线速度和角速度Type of args[0]: " << age0.type().name());
                            ROS_INFO_STREAM("2.创建Twist消息并设置线速度和角速度Type of args[1]: " << age1.type().name());
                            ROS_INFO_STREAM("2.创建Twist消息并设置线速度和角速度Type of args[2]: " << age2.type().name());
                            ROS_INFO_STREAM("2.创建Twist消息并设置线速度和角速度Type of args[3]: " << age3.type().name());
                            std::cout << "2.获取当前状态的成员函数 current_state: " << current_state << std::endl;

                            //恢复代价地图
                            std_srvs::Empty srv;
                            clear_costmaps_client_.call(srv);

                            if(current_state=="executing" && current_string=="navigate_to_goalpath"){
                                //切换到翻转路径
                                state_machine_.set_state("navigate_to_goalpathOverturn", {start_pose,target_pose, target_pose_default,std::string("navigate_to_goalpathOverturnAain"),plan_bendi});
                            }else if( current_state=="executing" && current_string=="navigate_to_goalpathOverturn"){
                                //切换到翻转路径
                                state_machine_.set_state("navigate_to_goalpath", {start_pose,target_pose, target_pose_default,std::string("navigate_to_goalpathAain"),plan_bendi});
                                std::cout << "/++++++++++++++error g  起始点 在 结束点 的后面，小车没法倒着走 ；只有一条路径,起始点和结束点都是当前路径的起始点和结束点 etShortestPath（） 没找到路径，路径状态不对   current_state:"<<current_state << std::endl;
                            }



                        } catch (const std::exception& e) {
                            ROS_ERROR("exception 切换到翻转路径 getShortestPath(): %s", e.what());
                        } catch (...) {
                            ROS_ERROR("exception 切换到翻转路径 getShortestPath()");
                        }


                    }else{
                        std::cout << "/+++++++++++++++///error 2: 起始点 在 结束点 的后面，小车没法倒着走 ；只有一条路径,起始点和结束点都是当前路径的起始点和结束点  start_result_index:"<< std::to_string(start_result_index) << std::endl;
                        std::cout << "/+++++++++++++++///error 2: 起始点 在 结束点 的后面，小车没法倒着走 ；只有一条路径,起始点和结束点都是当前路径的起始点和结束点  goal_result_index:"<< std::to_string(goal_result_index) << std::endl;

                    }


                    std::cout << "/+++++++++++++++///error 2: 起始点 在 结束点 的后面，小车没法倒着走 ；只有一条路径,起始点和结束点都是当前路径的起始点和结束点  start_result_index:"<< std::to_string(start_result_index) << std::endl;
                    std::cout << "/+++++++++++++++///error 2: 起始点 在 结束点 的后面，小车没法倒着走 ；只有一条路径,起始点和结束点都是当前路径的起始点和结束点  goal_result_index:"<< std::to_string(goal_result_index) << std::endl;

                    return false;
                }


                std::cout<<" 7:只有一条路径,起始点和结束点都是当前路径的起始点和结束点plan.size(): "<< plan.size()<< std::endl;
                std::cout<<" 7.1:判断起始切入点是否在当前的路径上,并获得列表对应的id，返回的在std::vector<int> plan_indexs 列表de索引  start_result_index :" <<std::to_string(start_result_index)<<"  : goal_result_index:  "<<std::to_string(goal_result_index)<< std::endl;
                std::cout<<" 7.2:判断起始切入点是否在当前的路径上   current_plan.size() :" <<std::to_string(current_plan.size())<< std::endl;


                if(goal_result_index>current_plan.size()||start_result_index>current_plan.size()) {
                         std::cout<<" 7.3:判断起始切入点是否在当前的路径上   current_plan.size() :" <<std::to_string(current_plan.size())<< std::endl;
                         std::cout<<" 7.4:判断起始切入点超出范围 current_plan.size()  start_result_index :" <<std::to_string(start_result_index)<<"  : goal_result_index:  "<<std::to_string(goal_result_index)<< std::endl;

                    return false;
                }

                 //todo 2.组成完整的路径 【（只有一条路径）加入第一条路径起始点 】

                vector<geometry_msgs::PoseStamped>::const_iterator Fist11 = current_plan.begin() +   start_result_index; // 找到第二个迭代器
                vector<geometry_msgs::PoseStamped>::const_iterator Second11 = current_plan.begin() + goal_result_index; // 找到第三个迭代器
                vector<geometry_msgs::PoseStamped> distance_plan_all_vector_jubu2;
                distance_plan_all_vector_jubu2.assign(Fist11,Second11); //使用assign() 成员函数将Arrs对应位置的值存入Arrs2数组中
                plan.insert(plan.end(), distance_plan_all_vector_jubu2.begin(), distance_plan_all_vector_jubu2.end());




                //end 只有一条路径时不需要路径与路径链接 current_path_goTo_target_path_map_plan 储存路径到路径链接的容器
                std::cout<<" 8:end 只有一条路径时不需要路径与路径链接 current_path_goTo_target_path_map_plan 储存路径到路径链接的容器plan.size(): "<< plan.size()<< std::endl;

            }else{
                if(k==0){//起始点
//                    if(!start_index_is_index_in_planfig){
//                        std::cout<<"/+++++++++++++++///error 2:判断起始切入点是否在当前的路径上,不在结构化网格上     :"<<std::to_string(start_min_index)<< std::endl;
//
//                        return false;
//                    }

                    std::cout<<" 9: 组成完整的路径 （ 加入第一条路径起始点 ）plan.size(): "<< plan.size()<< std::endl;
                    std::cout<<" 9.1:判断起始切入点是否在当前的路径上,并获得列表对应的id，返回的在std::vector<int> plan_indexs 列表de索引  start_result_index :" <<std::to_string(start_result_index)<<"  : goal_result_index:  "<<std::to_string(goal_result_index)<< std::endl;
                    std::cout<<" 9.2:判断起始切入点是否在当前的路径上   current_plan.size() :" <<std::to_string(current_plan.size())<< std::endl;
                    //todo 2.组成完整的路径 （ 加入第一条路径起始点 ）
                   /**
                    vector<geometry_msgs::PoseStamped>::const_iterator Fist11 = current_plan.begin() + start_result_index; // 找到第二个迭代器
                    vector<geometry_msgs::PoseStamped>::const_iterator Second11 = current_plan.begin() ; // 找到第三个迭代器
                    vector<geometry_msgs::PoseStamped> distance_plan_all_vector_jubu2;
                    std::cout<<" 9.3:  start_result_index :" <<std::to_string(start_result_index)<<"  : goal_result_index:  "<<std::to_string(goal_result_index)<< std::endl;
                    distance_plan_all_vector_jubu2.assign(Fist11,Second11); //使用assign() 成员函数将Arrs对应位置的值存入Arrs2数组中
                    std::cout<<" 9.4:  distance_plan_all_vector_jubu2.size() :" <<std::to_string(distance_plan_all_vector_jubu2.size())<< std::endl;
                    plan.insert(plan.end(), distance_plan_all_vector_jubu2.begin(), distance_plan_all_vector_jubu2.end());
                    */

                    std::cout<<" 9.3:  start_result_index :" <<std::to_string(start_result_index)<<"  : goal_result_index:  "<<std::to_string(goal_result_index)<< std::endl;
                    // 假设我们有两个vector，source和destination
                    //std::vector<geometry_msgs::PoseStamped> subvector(current_plan.begin() + start_result_index, current_plan.begin() + 0);
                    std::vector<geometry_msgs::PoseStamped> subvector(current_plan.begin() + start_result_index, current_plan.end());
                    std::cout<<" 9.4:  subvector.size() :" <<std::to_string(subvector.size())<< std::endl;
                    // 将截取的数据拼接到destination后面
                    plan.insert(plan.end(), subvector.begin(), subvector.end());
                    std::cout<<" 9.4:  current_plan.size() :" <<std::to_string(current_plan.size())<< std::endl;
                    std::cout<<" 9.4:  distance_plan_all_vector.size() :" <<std::to_string(distance_plan_all_vector.size())<< std::endl;
                    std::cout<<" 9.4:  subvector.size() :" <<std::to_string(subvector.size())<< std::endl;
                    std::cout<<" 9.4:  plan.size() :" <<std::to_string(plan.size())<< std::endl;

                    std::cout<<" 10:一条路径的终点会链接另一条路径的起点 【每一条路径都会与下一条路径相链接】plan.size(): "<< plan.size()<< std::endl;
                    std::cout<<" 10.1:判断起始切入点是否在当前的路径上,并获得列表对应的id，返回的在std::vector<int> plan_indexs 列表de索引  start_result_index :" <<std::to_string(start_result_index)<<"  : goal_result_index:  "<<std::to_string(goal_result_index)<< std::endl;
                    std::cout<<" 10.2:判断起始切入点是否在当前的路径上   current_plan.size() :" <<std::to_string(current_plan.size())<< std::endl;
                    std::cout<<" 10.3:一条路径的终点会链接另一条路径的起点 【每一条路径都会与下一条路径相链接】  shortest_path_ids[k] :" <<std::to_string(shortest_path_ids[k])<< std::endl;
                    std::cout<<" 10.2:一条路径的终点会链接另一条路径的起点   current_path_goTo_target_path_map_plan.size() :" <<std::to_string(current_path_goTo_target_path_map_plan.size())<< std::endl;


                    std::cout <<"path_id: "<< path_id << std::endl;


                    //todo 一条路径的终点会链接另一条路径的起点 【每一条路径都会与下一条路径相链接】

                      //---------------------------
                    //todo test 打印 debug 用的
                    map<int, MorPlanToMap>::iterator iter; //迭代器 test
                    for(iter = execute_map_plan_.begin(); iter != execute_map_plan_.end(); iter++) {
                        int current_plan_id = iter->first; //当前路径id，唯一id ；每个路径在数据库中都会存储一个唯一的id
                        cout<< "test execute_map_plan_ 当前路径的 current_plan_id : "<< current_plan_id << endl;
                    }

                    map<int, map<int, CurrentPathGoToTargetPath>>::iterator iter1; //迭代器 test
                    for(iter1 = current_path_goTo_target_path_map_plan.begin(); iter1 != current_path_goTo_target_path_map_plan.end(); iter1++) {
                        int current_plan_id = iter1->first; //当前路径id，唯一id ；每个路径在数据库中都会存储一个唯一的id
                        cout<< "test 一条路径的终点会链接另一条路径的起点 current_path_goTo_target_path_map_plan 当前路径的 current_plan_id : "<< current_plan_id << endl;

                        cout<< "222 test 一条路径的终点会链接另一条路径的起点 current_path_goTo_target_path_map_plan 当前路径的父亲 current_plan_id : "<< current_plan_id << endl;
                        map<int, CurrentPathGoToTargetPath>::iterator iter2; //迭代器 test
                        map<int, CurrentPathGoToTargetPath> target_map_plan =   current_path_goTo_target_path_map_plan[current_plan_id];
                        for(iter2 = target_map_plan.begin(); iter2 != target_map_plan.end(); iter2++) {
                            int current_plan_id2 = iter2->first; //当前路径id，唯一id ；每个路径在数据库中都会存储一个唯一的id
                            cout<< "333 test 一条路径的终点会链接另一条路径的起点 target_map_plan 当前路径的子 id current_plan_id2 : "<< current_plan_id2 << endl;
                        }
                    }
                    // end  test 打印 debug 用的


                      //--------------------------
                        //查找键值为k的元素，判断map中是否存在键值为k的元素
                        auto posKey=current_path_goTo_target_path_map_plan.find(shortest_path_ids[k]);
                        if(posKey!=current_path_goTo_target_path_map_plan.end()){
                            cout<<"10.4 一条路径的终点会链接另一条路径的起点 posKey->first:"<<posKey->first<<endl;
                            cout<<"10.4 一条路径的终点会链接另一条路径的起点 shortest_path_ids[k]:"<<shortest_path_ids[k]<<endl;
                            cout<<"10.4 一条路径的终点会链接另一条路径的起点 shortest_path_ids[k+1]:"<<shortest_path_ids[k+1]<<endl;
                            map<int, CurrentPathGoToTargetPath> target_map_plan =   current_path_goTo_target_path_map_plan[shortest_path_ids[k]];
                            auto posKey1=target_map_plan.find(shortest_path_ids[k+1]);
                            if(posKey1!=target_map_plan.end()){
                                CurrentPathGoToTargetPath currentPathGoToTargetPath =   target_map_plan[shortest_path_ids[k+1]];
                                std::vector<geometry_msgs::PoseStamped> currentPath   =currentPathGoToTargetPath.plan;
                                //组成完整的路径
                                plan.insert(plan.end(), currentPath.begin(), currentPath.end());
                            }else{
                                std::cout<<"/+++++++++++++++///error 10.4:一条路径的终点会链接另一条路径的起点 current_path_goTo_target_path_map_plan 无数据  start_min_index   :"<<std::to_string(start_min_index)<< std::endl;
                                std::cout<<"/+++++++++++++++///error 10.4:一条路径的终点会链接另一条路径的起点 current_path_goTo_target_path_map_plan 无数据  shortest_path_ids[k+1]   :"<<std::to_string(shortest_path_ids[k+1])<< std::endl;
                                return false;
                            }//if(posKey1!=target_map_plan.end())
                        }else{
                            std::cout<<"/+++++++++++++++///error 10.5:一条路径的终点会链接另一条路径的起点 current_path_goTo_target_path_map_plan 无数据  start_min_index    :"<<std::to_string(start_min_index)<< std::endl;
                            std::cout<<"/+++++++++++++++///error 10.5:一条路径的终点会链接另一条路径的起点 current_path_goTo_target_path_map_plan 无数据  shortest_path_ids[k]   :"<<std::to_string(shortest_path_ids[k])<< std::endl;

                            return false;
                        }//if(posKey!=current_path_goTo_target_path_map_plan.end())





                }else if(k==shortest_path_ids.size()-1){//结束点

//                    if(!goal_index_is_index_in_planfig){
//                        std::cout<<"/+++++++++++++++///error 3:判断终点切出点是否在当前的路径上 不在结构化网格上     :"<<std::to_string(start_min_index)<< std::endl;
//                        return false;
//                    }

                    std::cout<<" 11:组成完整的路径 （ 加入最后路径 结束点 ）plan.size(): "<< plan.size()<< std::endl;
                    std::cout<<" 11.1:判断起始切入点是否在当前的路径上,并获得列表对应的id，返回的在std::vector<int> plan_indexs 列表de索引  start_result_index :" <<std::to_string(start_result_index)<<"  : goal_result_index:  "<<std::to_string(goal_result_index)<< std::endl;
                    std::cout<<" 11.2:判断起始切入点是否在当前的路径上   current_plan.size() :" <<std::to_string(current_plan.size())<< std::endl;
                    //todo 3.组成完整的路径 （ 加入最后路径 结束点 ）
                    vector<geometry_msgs::PoseStamped>::const_iterator Fist11 = current_plan.begin() + 0; // 找到第二个迭代器
                    vector<geometry_msgs::PoseStamped>::const_iterator Second11 = current_plan.begin()+goal_result_index ; // 找到第三个迭代器
                    vector<geometry_msgs::PoseStamped> distance_plan_all_vector_jubu2;
                    distance_plan_all_vector_jubu2.assign(Fist11,Second11); //使用assign() 成员函数将Arrs对应位置的值存入Arrs2数组中
                    plan.insert(plan.end(), distance_plan_all_vector_jubu2.begin(), distance_plan_all_vector_jubu2.end());

                }else{//中间点

                    std::cout<<" 12:组成完整的路径 中间点"<< std::endl;

                    plan.insert(plan.end(), current_plan.begin(), current_plan.end());
                    //todo 2.组成完整的路径 （ 加入中间其它的路径 ）
                    if(k<shortest_path_ids.size()-1){ //【每一条路径都会与下一条路径相链接】

                        std::cout<<" 13:组成完整的路径 一条路径的终点会链接另一条路径的起点plan.size(): "<< plan.size()<< std::endl;
                        std::cout<<" 13.1:判断起始切入点是否在当前的路径上,并获得列表对应的id，返回的在std::vector<int> plan_indexs 列表de索引  start_result_index :" <<std::to_string(start_result_index)<<"  : goal_result_index:  "<<std::to_string(goal_result_index)<< std::endl;
                        std::cout<<" 13.2:判断起始切入点是否在当前的路径上   current_plan.size() :" <<std::to_string(current_plan.size())<< std::endl;
                        //todo 一条路径的终点会链接另一条路径的起点
                        //查找键值为k的元素，判断map中是否存在键值为k的元素
                        auto posKey=current_path_goTo_target_path_map_plan.find(shortest_path_ids[k]);
                        if(posKey!=current_path_goTo_target_path_map_plan.end()){
                            cout<<"10.t 一条路径的终点会链接另一条路径的起点 posKey->first:"<<posKey->first<<endl;
                            cout<<"10.t 一条路径的终点会链接另一条路径的起点 shortest_path_ids[k]:"<<shortest_path_ids[k]<<endl;
                            cout<<"10.t 一条路径的终点会链接另一条路径的起点 shortest_path_ids[k+1]:"<<shortest_path_ids[k+1]<<endl;
                            map<int, CurrentPathGoToTargetPath> target_map_plan =   current_path_goTo_target_path_map_plan[shortest_path_ids[k]];
                            auto posKey1=target_map_plan.find(shortest_path_ids[k+1]);
                            if(posKey1!=target_map_plan.end()){
                                CurrentPathGoToTargetPath currentPathGoToTargetPath =   target_map_plan[shortest_path_ids[k+1]];
                                std::vector<geometry_msgs::PoseStamped> currentPath   =currentPathGoToTargetPath.plan;
                                //组成完整的路径
                                plan.insert(plan.end(), currentPath.begin(), currentPath.end());
                            }else{
                                std::cout<<"/+++++++++++++++///error 1:一条路径的终点会链接另一条路径的起点 current_path_goTo_target_path_map_plan 无数据     :"<<std::to_string(start_min_index)<< std::endl;
                                std::cout<<"/+++++++++++++++///error 1:一条路径的终点会链接另一条路径的起点 current_path_goTo_target_path_map_plan 无数据   shortest_path_ids[k+1]  :"<<std::to_string(shortest_path_ids[k+1])<< std::endl;
                                return false;
                            }
                        }else{
                            std::cout<<"/+++++++++++++++///error 2:一条路径的终点会链接另一条路径的起点 current_path_goTo_target_path_map_plan 无数据      :"<<std::to_string(start_min_index)<< std::endl;
                            std::cout<<"/+++++++++++++++///error 2:一条路径的终点会链接另一条路径的起点 current_path_goTo_target_path_map_plan 无数据   shortest_path_ids[k]  :"<<std::to_string(shortest_path_ids[k])<< std::endl;

                            return false;
                        }
                    }//-----------------结束if(k<shortest_path_ids.size()-1)语句-----------------

                }//-----------------结束if语句-----------------
            }//-----------------结束if语句-----------------


        }//结束for循环


        std::cout<<" 14.1:获取半结构全局规划路径   起始点 切入点索引 start_min_index :"<<std::to_string(start_min_index)<< std::endl;
        std::cout<<" 14.1:获取半结构全局规划路径   结束点 切出点索引 goal_min_index :"<<std::to_string(goal_min_index)<< std::endl;

        std::cout<<" 14.2:获取半结构全局规划路径   起始点 坐标 start_point.x :"<<std::to_string(start_point.x)<<" :start_point.y:  "<<std::to_string(start_point.y)<< std::endl;
        std::cout<<" 14.2:获取半结构全局规划路径   结束点 坐标 goal_point.x :" <<std::to_string(goal_point.x)<<"  : goal_point.y:  "<<std::to_string(goal_point.y)<< std::endl;

        std::cout<<" 14.3:获取半结构全局规划路径   最近切入点  坐标 start_min_point.x :"<<std::to_string(start_min_point.x)<<" : start_min_point.y:  "<<std::to_string(start_min_point.y)<< std::endl;
        std::cout<<" 14.3:获取半结构全局规划路径   最近切出点  坐标 goal_min_point.x :" <<std::to_string(goal_min_point.x)<<"  : goal_min_point.y:  "<<std::to_string(goal_min_point.y)<< std::endl;



        std::cout<<" 14:组成完整的路径 切出点到终点的路径plan.size(): "<< plan.size()<<std::endl;
        //todo 3.切出点到终点的路径
        std::vector<geometry_msgs::PoseStamped> goalplan;
        std::vector<Point> goalplan_points;
        //全局规划 algorithm= Dijkstra 算法 ; AStar 算法   ，goal_min_point：切出点，goal_point：结束点
        bool  goalpathfig=makePlanWithSemiStructuredPath2(goal_min_point,goal_point, goalplan,goalplan_points,"AStar");
        if(!goalpathfig){
            std::cout<<"/+++++++++++++++///error 5:切出点到终点的路径规划失败     :"<<std::to_string(start_min_index)<< std::endl;
            return false;
        }

        std::cout<<" 15:组成完整的路径 切出点到终点的路径plan.size(): "<< plan.size()<< std::endl;
        //组成完整的路径
        plan.insert(plan.end(), goalplan.begin(), goalplan.end());

        std::cout<<" 16:组成完整的路径 切出点到终点的路径 plan.size(): "<< plan.size()<<std::endl;
        //--------------------------------------------
        /**
        //获取小车当前位置
        geometry_msgs::PoseStamped robot_pose;
        if(!getRobotPose(robot_pose)){
            ROS_ERROR("getShortestPath cannot make a plan for you because it could not get the start pose of the robot");
            //日志
            //  contnav_error(__FILE__,__LINE__,const_cast<char*>(response.message.c_str()));
            //  contnav_error(__FILE__,__LINE__,"global_planner::goalCB 获取小车当前位置 error");
            return false;
        }
        geometry_msgs::PoseStamped start_pose;
        //起始位置
        start_pose.header.stamp = ros::Time::now();
        start_pose.header.frame_id=frame_id_;
        start_pose.pose = robot_pose.pose;

        // add orientations if needed 如果需要，添加方向
        orientation_filter_->processPath(start_pose, goalplan);

        //publish the plan for visualization purposes 出于可视化目的发布计划
        nav_msgs::Path gui_path;
        publishPlan(plan,gui_path);
        pathPlan_semiStructured_pub_.publish(gui_path);

        //---------------------------------------
        // 输出所有路径
        for (std::vector<int> path : all_paths) {
            std::cout << "Path: ";
            for (int node_id : path) {
                std::cout << node_id << "->";
            }
            std::cout << "End\n";
        }

        // 输出最短路径
        std::cout << "输出最短路径 Shortest path: ";
        for (int node_id : all_paths[all_paths_vector_min_index]) {
            std::cout << node_id << "->";
        }
        std::cout << "End\n";
        */





        //----
        return true;

    }

    //读取文件
    std::vector<geometry_msgs::PoseStamped> global_planner::readEulerAnglesFromFile(const std::string& filename)
    {
        std::vector<geometry_msgs::PoseStamped> plan;
        std::ifstream infile(filename.c_str());
        if (!infile.is_open())
        {
            std::cerr << "Failed to open file: " << filename << std::endl;
            return plan;
        }

        geometry_msgs::PoseStamped pose;
        while (infile >> pose.pose.position.x >> pose.pose.position.y >> pose.pose.position.z)
        {
            double yaw;
            infile >> yaw;

            tf2::Quaternion quat;
            quat.setRPY(0.0, 0.0, yaw);

            pose.pose.orientation = tf2::toMsg(quat);
            plan.push_back(pose);
        }

        infile.close();

        return plan;
    }

    //写入文件
    void global_planner::writeEulerAnglesToFile(const std::vector<geometry_msgs::PoseStamped>& plan, const std::string& filename)
    {
        std::ofstream outfile;
        outfile.open(filename.c_str());

        if (!outfile.is_open())
        {
            std::cerr << "写入文件 Failed to open file: " << filename << std::endl;
            return;
        }

        for (const auto& pose : plan)
        {
            tf2::Quaternion quat;
            tf2::fromMsg(pose.pose.orientation, quat);

            double roll, pitch, yaw;
            tf2::Matrix3x3(quat).getRPY(roll, pitch, yaw);

            outfile << pose.pose.position.x << " " << pose.pose.position.y << " "
                    << pose.pose.position.z << " " << yaw << std::endl;
        }

        outfile.close();

        std::cout << "写入文件 Euler angles have been saved to " << filename << std::endl;
    }
    //创建文件路径
    void global_planner::createDirectory(const std::string& path)
    {
        struct stat info;

        if (stat(path.c_str(), &info) != 0)
        {
            // 如果路径不存在，则创建路径
            int status = mkdir(path.c_str(), S_IRWXU | S_IRWXG | S_IROTH | S_IXOTH);

            if (status != 0)
            {
                std::cerr << "如果路径不存在，则创建路径 Failed to create directory: " << path << std::endl;
            }
        }
    }



    //todo 2.点到点 规划路径，半结构路径规划中的 makePlan函数   tolerance =default_tolerance_ //    1451    private_nh2.param("default_tolerance", default_tolerance_, 0.0);//#默认0.0，目标容差
    bool global_planner::makePlanWithSemiStructuredPath(Point & startpoint, Point &target_goal_point,string algorithm,
                                  double tolerance, std::vector<geometry_msgs::PoseStamped>& plan) {
        boost::mutex::scoped_lock lock(mutex2_);
        if(!initialized_||!grid_ready_||!grid_updated_ready_){
            ROS_ERROR("1.makePlanWithSemiStructuredPath 规划路径，半结构路径规划中的 makePlanWithSemiStructuredPath 函数 未初始化  The planner has not been initialized, please call initialize() to use the planner");
            //日志
            //  contnav_error(__FILE__,__LINE__,const_cast<char*>(response.message.c_str()));
            //  contnav_error(__FILE__,__LINE__,"3.路径规划 未初始化 The planner has not been initialized, please call initialize() to use the planner");
            return false;
        }
        unsigned int cx = origin_costmap_->getSizeInCellsX(), cy = origin_costmap_->getSizeInCellsY();
        std::cout << "2.makePlanWithSemiStructuredPath 路径规划 ================================================== " << std::endl;

        //todo 2.选择规划算法
        if(algorithm=="Dijkstra"){//Dijkstra
            std::cout << "3.makePlanWithSemiStructuredPath 路径规划 makePlan Dijkstra 我的全局规划路径 initialize " << std::endl;
            if(planner_!=NULL)
                delete planner_;
            DijkstraExpansion* de = new DijkstraExpansion(p_calc_, cx, cy);//true
            de->setPreciseStart(true);
            planner_ = de;
        }else if(algorithm=="AStar"){//AStar
            std::cout << "4.makePlanWithSemiStructuredPath 路径规划 makePlan AStar 我的全局规划路径 initialize " << std::endl;
            if(planner_!=NULL)
                delete planner_;
            planner_ = new AStarExpansion(p_calc_, cx, cy);//false  // cx, cy 地图的大小，即像素数。
        }else {

            std::cout << "5.makePlanWithSemiStructuredPath error 路径规划 未选择规划算法！ " << std::endl;
            return false;
        }


        //clear the plan, just in case  清除计划，以防万一
        plan.clear();


        unsigned int start_x_i, start_y_i, goal_x_i, goal_y_i;
        double start_x, start_y, goal_x, goal_y;



        start_x=startpoint.x;
        start_y=startpoint.y;
        goal_x=target_goal_point.x;
        goal_y=target_goal_point.y;

        start_x_i=(unsigned int)start_x;
        start_y_i=(unsigned int)start_y;
        goal_x_i=(unsigned int)goal_x;
        goal_y_i=(unsigned int)goal_y;

        std::cout << "5.makePlanWithSemiStructuredPath 起始点坐标 start_x： " <<to_string(start_x)<<" , start_y： " <<to_string(start_y)<<std::endl;
        std::cout << "6.makePlanWithSemiStructuredPath 终点坐标 goal_x： " <<to_string(goal_x)<<" , goal_y： " <<to_string(goal_y)<<std::endl;



        //clear the starting cell within the costmap because we know it can't be an obstacle 清除成本图中的起始单元格，因为我们知道它不会成为障碍
        //  clearRobotCell(start, start_x_i, start_y_i);
       //void global_planner::clearRobotCell(const geometry_msgs::PoseStamped& global_pose, unsigned int mx, unsigned int my)
        //set the associated costs in the cost map to be free
        origin_costmap_->setCost(start_x_i, start_y_i, costmap_2d::FREE_SPACE);//自由空间

        //setSize( cx_,  cy_);
        //make sure to resize the underlying array that Navfn uses 确保调整Navfn使用的基础数组的大小
        p_calc_->setSize(nx_, ny_);
        planner_->setSize(nx_, ny_);
        potential_ = new float[nx_ * ny_]; //地图数组存代价直

        std::cout << "7.makePlan 终点坐标 nx_： " <<to_string(nx_)<<" , ny_： " <<to_string(ny_)<<std::endl;
        /**
        具体而言，该函数的实现方式是，分别用指针 pc 指向数组的第一行、最后一行、第一列和最后一列，
        然后使用循环依次遍历这四个边界，并将它们的每个元素设置为指定的 value 值。
       在代码的实现过程中，nx 和 ny 分别表示数组 costarr 的宽度和高度，unsigned char 是一个无符号字符类型，用
        于存储 8 位无符号整数。因此，该函数的输入参数包括一个指向无符号字符数组的指针 costarr，数组的宽度 nx 和高度 ny，以及要设置的值 value。
         */

        /**
              static const unsigned char NO_INFORMATION = 255;
              static const unsigned char LETHAL_OBSTACLE = 254;
              static const unsigned char INSCRIBED_INFLATED_OBSTACLE = 253;
              static const unsigned char FREE_SPACE = 0;
              */

        if(outline_map_)
            outlineMap(origin_costmap_->getCharMap(), nx_, ny_, costmap_2d::LETHAL_OBSTACLE);  //costmap_2d::LETHAL_OBSTACLE = 254表示在此单元为会发生碰撞的障碍物。


        ///home/sukai/workspace/xju-robot_gai/src/thirdparty/navigation/global_planner/src/astar.cpp 53
        ///home/sukai/workspace/xju-robot_gai/src/thirdparty/navigation/global_planner/src/dijkstra.cpp 80
        //planner_- 用a* 还是 dijkstra 做全局规划 175
        /**
         * //potential（预估每个点到目标节点的距离）
         * 这段代码是在进行路径规划，其中调用了planner_的calculatePotentials函数，该函数的作用是计算出从起点到终点的所有路径的代价值，即potential_array_数组。其中，costmap_->getCharMap()是获取地图的信息，
         * start_x, start_y, goal_x, goal_y是起点和终点的坐标，nx * ny * 2是数组potential_array_的大小。
         * 函数返回值是一个bool类型的变量found_legal，表示是否找到了一条合法的路径。
         *
         * 在这段代码中，costmap_->getCharMap()返回的是一个unsigned char类型的指针，指向的是一个二维数组，数组中的每个元素表示对应位置的cell的状态。在这个数组中，0表示自由空间，255表示障碍物，其他值表示不同的占用程度。
         * 对不起，我的之前的回答有误。在ROS中，costmap_->getCharMap() 返回的数据类型是指向 unsigned char 类型的一维数组的指针。这个一维数组的长度等于地图的宽度乘以高度。每个数组元素对应着地图中一个单元格，其值表示该单元格的状态。
         * 通常，0 表示自由空间，100 表示障碍物，而其他的值则可以用于表示其他状态。
         *
         * nx * ny * 2
        这个参数是用来指定potential_array_数组的大小的，这个数组是用来存储每个点到目标点的距离的。在这个函数中，potential_array_数组的大小是nx * ny * 2，
         其中nx和ny是地图的宽度和高度，2是因为在计算距离时，需要同时计算正向和反向的距离，因此数组的大小需要乘以2。在这个函数中，nx和ny的值是通过costmap_的getSizeInCellsX()和getSizeInCellsY()函数获取的，
         这两个函数返回的是地图的宽度和高度，单位是cell。因此，nx * ny * 2实际上是地图中所有cell的数量乘以2。
         */
        bool found_legal = planner_->calculatePotentials(origin_costmap_->getCharMap(), start_x, start_y, goal_x, goal_y,
                                                         nx_ * ny_ * 2, potential_);//float* potential_array_; 该函数的作用是计算出从起点到终点的所有路径的代价值，即potential_array_数组

//        if(!old_navfn_behavior_)
//            planner_->clearEndpoint(costmap_->getCharMap(), potential_, goal_x_i, goal_y_i, 2);


        /**
         * 这个函数与getPlanFromPotential(start_x, start_y, goal_x, goal_y, goal, plan)函数的区别在于，getPlanFromPotential函数是用于从潜在代价地图中获取路径的函数，
         * 而publishPotential函数是用于发布潜在代价地图的函数。getPlanFromPotential函数的输入是起点和终点的坐标，
         * 以及一个空的路径plan，函数的输出是一个bool类型的值，表示是否成功获取到路径。如果成功获取到路径，则路径会被存储在plan中。publishPotential函数的输入是一个浮点型数组，
         * 表示每个点到目标节点的距离，函数的输出是一个nav_msgs::OccupancyGrid类型的消息，表示整个代价地图。函数首先获取代价地图的大小和分辨率，然后将整个代价地图发布出去。
         * 在发布代价地图之前，函数会对代价地图进行处理，将代价地图中的每个点的值进行归一化处理，使得代价地图中的最大值为1，最小值为0。最后，函数将处理后的代价地图发布出去。
         */

        if(publish_potential_)
            publishPotential(potential_);

        if (found_legal) {//是否已经计算得到路径代价了


            std::cout << "7.makePlanWithSemiStructuredPath  planner_->calculatePotentials  已经计算得到代价值  " <<std::endl;

            //extract the plan
            /**
             * 这个函数是用来从potential_array_数组中获取一条从起点到终点的路径的。在这个函数中，start_x和start_y是起点的坐标，goal_x和goal_y是终点的坐标，
             * goal是一个geometry_msgs::PoseStamped类型的变量，表示终点的位置，plan是一个std::vector<geometry_msgs::PoseStamped>类型的变量，用来存储路径上的所有点。在函数中，
             * 首先会调用calculatePotentials()函数计算每个点到终点的距离，然后再从potential_array_ （代价）数组中获取一条从起点到终点的路径，并将路径上的所有点存储到plan变量中。


             */
           geometry_msgs::PoseStamped goal;//todo 给一个空的就可以了。getPlanFromPotential函数中会未使用这个参数。
            std::cout << "8.makePlanWithSemiStructuredPath  getPlanFromPotential 准备找 路径了 " <<std::endl;
            if (getPlanFromPotential(potential_,start_x, start_y, goal_x, goal_y, goal, plan)) {
                //make sure the goal we push on has the same timestamp as the rest of the plan 确保我们推进的目标与计划的其余部分具有相同的时间戳
//                geometry_msgs::PoseStamped goal_copy = goal;
//                goal_copy.header.stamp = ros::Time::now();
//                plan.push_back(goal_copy);
            } else {
                ROS_ERROR("error 未能从潜在客户处获取计划Failed to get a plan from potential when a legal potential was found. This shouldn't happen.");//发现合法潜在客户时，未能从潜在客户处获取计划。这不应该发生
            }
        }else{
            ROS_ERROR_THROTTLE(5.0, "error 无法获取计划 Failed to get a plan.");//无法获取计划
        }

        // add orientations if needed 如果需要，添加方向
      //  orientation_filter_->processPath(start, plan);//不需要方向了

        //publish the plan for visualization purposes 出于可视化目的发布计划
        nav_msgs::Path gui_path;
        publishPlan(plan,gui_path);
        plan_pub_.publish(gui_path);
        delete[] potential_;
        return !plan.empty();
    }




    /**
    bool global_planner::makePlan(const geometry_msgs::PoseStamped& start, const geometry_msgs::PoseStamped& goal, std::vector<geometry_msgs::PoseStamped>& plan)
    {

        boost::mutex::scoped_lock lock(mutex_);

        // ROS_WARN("1. makePlan my_global_planner_plugin 我的全局规划路径 makePlan ");
        //  std::cout << "1.makePlan my_global_planner_plugin 我的全局规划路径 makePlan " << std::endl;
        if (!initialized_) {
            ROS_ERROR(
                    "This planner has not been initialized yet, but it is being used, please call initialize() before use");
            return false;
        }
        // ROS_WARN("2.my_global_planner_plugin 我的全局规划路径 makePlan ");

        //clear the plan, just in case  清除计划，以防万一
        plan.clear();
        open_set_.clear();//待遍历的点
        ros::NodeHandle n;
        std::string global_frame = frame_id_;


        //直到tf能够处理改变过去的事情。。。我们将要求目标在我们的全球框架内
        //until tf can handle transforming things that are way in the past... we'll require the goal to be in our global frame
        if (goal.header.frame_id != global_frame) {
            ROS_ERROR(
                    "The goal pose passed to this planner must be in the %s frame.  It is instead in the %s frame.", global_frame.c_str(), goal.header.frame_id.c_str());
            return false;
        }
        if (start.header.frame_id != global_frame) {
            ROS_ERROR(
                    "The start pose passed to this planner must be in the %s frame.  It is instead in the %s frame.", global_frame.c_str(), start.header.frame_id.c_str());
            return false;
        }
        //起始点坐标
        double s_wx = start.pose.position.x;
        double s_wy = start.pose.position.y;
        //目标点坐标
        double g_wx = goal.pose.position.x;
        double g_wy = goal.pose.position.y;

        unsigned int start_x_i, start_y_i, goal_x_i, goal_y_i;
        //像素坐标
        int start_x, start_y, goal_x, goal_y;

        if (!costmap_->worldToMap(s_wx, s_wy, start_x_i, start_y_i)) {//世界坐标转像素坐标
            //机器人的起始位置不在全球成本图上。计划总是会失败，你确定机器人已经正确本地化了吗？
            ROS_WARN(
                    "The robot's start position is off the global costmap. Planning will always fail, are you sure the robot has been properly localized?");
            return false;
        }
        if (!costmap_->worldToMap(g_wx, g_wy, goal_x_i, goal_y_i)) {
            ROS_WARN_THROTTLE(1.0,
                              "The goal sent to the global planner is off the global costmap. Planning will always fail to this goal.");
            return false;
        }
        //像素坐标
        start_x = start_x_i;
        start_y = start_y_i;
        goal_x = goal_x_i;
        goal_y = goal_y_i;

        //清除成本图中的起始单元格，因为我们知道它不会成为障碍  clear the starting cell within the costmap because we know it can't be an obstacle
        clearRobotCell(start, start_x_i, start_y_i);
        //nx 和 ny 分别表示数组 地图 的宽度和高度
        int nx = costmap_->getSizeInCellsX(), ny = costmap_->getSizeInCellsY();
        setSize( nx,  ny);
        //costmap_->getCharMap() : costarr 是一个一维数组，用于存储地图的成本值。它的大小为 nx * ny。
        outlineMap(costmap_->getCharMap(), nx, ny, costmap_2d::LETHAL_OBSTACLE);  //costmap_2d::LETHAL_OBSTACLE = 254表示在此单元为会发生碰撞的障碍物。

        unsigned char* costarr = costmap_->getCharMap(); //costarr 是一个一维数组，用于存储地图的成本值。它的大小为 nx * ny。
        int start_index = toIndex(start_x, start_y,nx,ny);// 计算起点在一维数组中的下标。// 将起点坐标 start_x 和 start_y 转化为一维数组中的索引 start_index。  x + nx * y   =139+384*219=84235
        int goal_index  = toIndex(goal_x, goal_y,nx, ny);//计算终点在一维数组中的下标。 //将终点坐标 goal_x 和 goal_y 转化为一维数组中的索引 goal_index。
        std::cout<<"22start_index:"<<std::to_string(start_index)<< std::endl;
        std::cout<<"22goal_index:"<<std::to_string(goal_index)<< std::endl;
        queue_.clear();//清空一个存放优先级队列的vector容器queue_。
        queue_.push_back(Index(start_index, 0));//将起点压入优先级队列，优先级为0。//将起点 start_i 加入到优先队列 queue_ 中，第二个参数 0 表示起点的势能 代价值为 0。//将势能数组 potential 的所有元素都赋值为 POT_HIGH，表示所有未探索的空间的势能都是无穷大。

        potential_ = new float[nx * ny];
        std::cout<<"--------------aaa-------------"<< std::endl;
        open_set_array_ = new Index[nx * ny]();
        std::cout<<"--------------bbb------------"<< std::endl;


        // int length2 = sizeof(potential_) / sizeof(float);
        int length2 = sizeof(potential_) / sizeof(potential_[0]);
        //float* potential;数据全部填充为 POT_HIGH 1.0e10  最大值
        std::fill(potential_, potential_ + ns_, POT_HIGH);//初始化一个数组potential，用于存储每个格子的路径潜力值，将其全部赋值为一个较大的值POT_HIGH。 #define POT_HIGH 1.0e10 表示所有未探索的空间的势能都是无穷大。
        potential_[start_index] = 0;//将起点的路径潜力值设置为0。// 将起点的势能设为 0。
        open_set_array_[start_index] = Index(start_index, 0,start_index,start_x,start_y );//将起点的路径潜力值设置为0。// 将起点的势能设为 0。
        int cycle = 0;  //设置循环次数 cycle 为 0。
        int  cycles=nx * ny * 2;//设置循环次数 cycles 为 nx * ny * 2。 2是因为在计算距离时，需要同时计算正向和反向的距离，所以乘以2。
        unsigned char* costs = costmap_->getCharMap();
        while (queue_.size() > 0 && cycle < cycles) {//循环条件为队列不为空且循环次数小于cycles=nx * ny * 2 。 // 当优先队列不为空且循环次数小于 cycles 时，进行循环。
            Index top = queue_[0];//Index top = queue_[0];：获取队列中优先级最高的元素。 // 取出优先队列中的第一个元素（最小势能值），保存到 top 中。
            std::pop_heap(queue_.begin(), queue_.end(), greater1());//将队列中的元素重新排列，使得优先级最高的元素被移到最后一个位置。 //弹出优先队列中的最小元素，并保持堆的性质。
            //把代价最小的数据加入open_set_ 待遍历的点中
            //open_set_.push_back(queue_[0]);
            //open_set_.sort();
            //open_set_.sort(greater1());
            queue_.pop_back();//：弹出队列中的最后一个元素，即优先级最高的元素。 // 删除队列的最后一个元素。
            //queue_.clear();//清空一个存放优先级队列的vector容器queue_。

            int i = top.i;//获取弹出的元素的下标。//取出 top 中保存的元素的索引值 i。
//          std::cout<<"top.i:"<<std::to_string(top.i)<< std::endl;
//          std::cout<<"goal_index:"<<std::to_string(goal_index)<< std::endl;
            //如果弹出的元素是终点，则返回true表示找到了最短路径。 //如果当前位置 i 是终点，则找到了一条通路，直接返回 true。
            if (i == goal_index)  {
                ROS_DEBUG("Dijkstra: found path to goal");
                std::cout<<"************************结束*************************************************************************************************************************************** "<< goal_index <<std::endl;

                std::cout<<"***如果弹出的元素是终点，跳出循环 表示找到了最短路径** goal_index: "<< goal_index <<std::endl;
                std::cout<<"***std::to_string(potential_[i]): "<< std::to_string(potential_[i]) <<" i: "<<i<<std::endl;
                std::cout<<"***std::to_string(potential_[i+1]): "<< std::to_string(potential_[i+1]) <<" i: "<<i+1<<std::endl;
                std::cout<<"***std::to_string(potential_[i-1]): "<< std::to_string(potential_[i-1]) <<" i: "<<i-1<<std::endl;
                std::cout<<"***std::to_string(potential_[i+ nx_]): "<< std::to_string(potential_[i+ nx_]) <<" i: "<<i+ nx_<<std::endl;
                std::cout<<"***std::to_string(potential_[i- nx_]): "<< std::to_string(potential_[i- nx_]) <<" i: "<<i- nx_<<std::endl;
                break;
            }



//          std::cout<<"22222222222222potential->2222222222222 length2: "<<length2<< std::endl;
//          std::cout<<"nx:"<<std::to_string(nx)<< std::endl;
//          std::cout<<"ny:"<<std::to_string(ny)<< std::endl;
//          std::cout<<"cycles:"<<std::to_string(cycles)<< std::endl;
//
//          std::cout<<"起始点坐标 s_wx:"<<std::to_string(s_wx)<< std::endl;
//          std::cout<<"s_wy:"<<std::to_string(s_wy)<< std::endl;
//          std::cout<<"起始点像素坐标 start_x:"<<std::to_string(start_x)<< std::endl;
//          std::cout<<"start_y:"<<std::to_string(start_y)<< std::endl;
//          std::cout<<"起始点索引： start_index:"<<std::to_string(start_index)<< std::endl;
//          std::cout<<"******************************potential_[i]:"<<std::to_string(potential_[i])<< std::endl;
            //costmap_->getCharMap() = costs；
            //  std::cout<<"add(costs  1111111111111111111111111111111111111111111111111111111111111111111111 "<< i <<std::endl;

            add(costs, potential_, i + 1,i, start_x, start_y ,goal_x, goal_y); //根据当前元素的下标，计算其相邻格子的路径潜力值并压入队列。// 调用 add 函数，将 i 的邻居节点 i+1 加入到队列中。
            //  std::cout<<"add(costs  222222222222222222222222222222222222222222222222222222222222222222222 "<< i <<std::endl;

            add(costs, potential_,  i - 1,i,   start_x, start_y,goal_x, goal_y);
            // std::cout<<"add(costs  333333333333333333333333333333333333333333333333333333333333333333333 "<< i <<std::endl;

            add(costs, potential_,  i + nx_,i,   start_x, start_y,goal_x, goal_y);
            // std::cout<<"add(costs  444444444444444444444444444444444444444444444444444444444444444444444 "<< i <<std::endl;

            add(costs, potential_, i - nx_,i,  start_x, start_y,goal_x, goal_y);



//          std::cout<<"******************************************开始*********************************************************goal_index "<< goal_index <<std::endl;
//          std::cout<<"******************************************开始*********************************************************i "<< i <<std::endl;
//          std::cout<<"******************************************开始*********************************************************start_index "<< start_index<<std::endl;
//
//
//          std::cout<<"***std::to_string(potential_[i]): "<< std::to_string(potential_[i]) <<" i: "<<i<<std::endl;
//          std::cout<<"***std::to_string(potential_[i+1]): "<< std::to_string(potential_[i+1]) <<" i: "<<i+1<<std::endl;
//          std::cout<<"***std::to_string(potential_[i-1]): "<< std::to_string(potential_[i-1]) <<" i: "<<i-1<<std::endl;
//          std::cout<<"***std::to_string(potential_[i+ nx_]): "<< std::to_string(potential_[i+ nx_]) <<" i: "<<i+ nx_<<std::endl;
//          std::cout<<"***std::to_string(potential_[i- nx_]): "<< std::to_string(potential_[i- nx_]) <<" i: "<<i- nx_<<std::endl;

            cycle++;//循环次数加1。//周期数加1，表示已经处理了一个周期的数据。
        }


        //=========================================================================
        //







//
        ////发布代价地图
        if(publish_potential_)
            publishPotential(potential_);

        //如果找到了最短路径，则将路径压入plan中。//如果找到了一条通路，将其保存到 plan 中。
        if (getPlanFromPotential(potential_,start_x, start_y, goal_x, goal_y, goal, plan)) {
            //make sure the goal we push on has the same timestamp as the rest of the plan 确保我们推进的目标与计划的其余部分具有相同的时间戳
            geometry_msgs::PoseStamped goal_copy = goal;
            goal_copy.header.stamp = ros::Time::now();
            plan.push_back(goal_copy);
            // add orientations if needed 如果需要，添加方向

            //如果需要，添加方向
            orientation_filter_->processPath(start, plan);
            //publish the plan for visualization purposes 出于可视化目的发布计划
            publishPlan(plan);
        } else {
            ROS_ERROR("Failed to get a plan from potential when a legal potential was found. This shouldn't happen.");//发现合法潜在客户时，未能从潜在客户处获取计划。这不应该发生

            return false;
        }
        //

        delete[] potential_;
        delete[] open_set_array_;
        return true;
    }
*/




    // todo 打印代价矩阵
    void global_planner::print_graph(MorPlanToMap *morPlanToMap_potential) {
        // 打印代价矩阵
        std::cout << "node cx_: " << cx_ << std::endl;
        std::cout << "node cy_: " << cy_ << std::endl;
        for (auto i = 0; i < cx_; ++i) std::cout << GREEN << std::setw(3) << i << " " << WHITE;
        std::cout << "\n";
        for (auto i = 0; i < cx_; ++i) {
            for (auto j = 0; j < cy_; ++j) {
             int index_jubu =   toIndex((int) i, (int)  j, nx_ , ny_);
                   // if (graph_[i][j] != std::numeric_limits<double>::max()) {
                    if (morPlanToMap_potential[index_jubu].cost!= std::numeric_limits<double>::max()) {
                    std::string str {"000 "};
                    auto gij = std::to_string(morPlanToMap_potential[index_jubu].cost);
                    for (auto k = 0; k < 3; ++k) {
                        if (gij.size() > k) str[k] = gij[k];
                    }
                    std::cout << str.c_str();
                } else {
                    std::cout << "*** ";
                }
            }
            std::cout << GREEN << " " << i << "\n" << WHITE;
        }
    }


    void global_planner::add(unsigned char* costs, float* potential, int next_i,int current_i, int start_x,int start_y, int end_x,int end_y) {

        //  std::cout<< "add 1111111111111111111111111  大于代价直 next_i: "<<next_i<<" ;ns: "<<std::to_string(nx_ * ny_)<<std::endl;


        //如果下标越界，则直接返回。  ns_ = nx * ny;
        int ns=nx_ * ny_;
        if (next_i < 0 || next_i >= ns)
            return;

        //  std::cout<< "add 222222222222222222222222222222  大于代价直 next_i: "<<next_i<<" ;potential[next_i]: "<<std::to_string(potential[next_i])<<std::endl;

        if (potential[next_i] < POT_HIGH)// 如果这个节点已经被处理过了，也就是它的 potential 值已经被赋值过了，直接返回。 [potential被赋值过了，这个值肯定小于POT_HIGH ，说明这个点已经被处理过了，不需要再处理了。 ]
            return;

        // std::cout<< "add 33333333333333333333333333333333  大于代价直 next_i: "<<next_i<<" ;costs[next_i]: "<<std::to_string(costs[next_i])<<std::endl;


//        std::cout<<"lethal_cost_:"<<std::to_string(lethal_cost_)<< std::endl;
//        std::cout<<"代价 costs[next_i]:"<<std::to_string(costs[next_i])<< std::endl;

        //costmap_->getCharMap() = costs；   lethal_cost_ = 253 致命代价值    //unknown_ 是否探索未知区域，flase--不可到达
        if(costs[next_i]>=lethal_cost_  && !(unknown_ && costs[next_i]==costmap_2d::NO_INFORMATION))//NO_INFORMATION = 255 未知 保留给没有足够信息的单元格。；如果该点的cost值超过lethal_cost_，或者未知但unknown_标志为false，也直接返回
        {

            //      std::cout<< "add  大于代价直 next_i: "<<next_i<<" ;costs[next_i]: "<<std::to_string(costs[next_i])<<std::endl;
            return;
        }

        //是否存在过
//    bool is_element_exist(int target_i, const std::vector<Index>& open_set_) {
//        for (const auto& elem : open_set_) {
//            if (elem.i == target_i) {
//                return true;
//            }
//        }
//        return false;
//    }


        //是否存在过
//    if(std::find_if(open_set_.begin(), open_set_.end(),[next_i](const Index& elem) { return elem.i == next_i; })!= open_set_.end()){
//        std::cout<<"已经存在过了"<< std::endl;
//        return;
//    }

        // std::cout<< "add 444444444444444444444444444444444  大于代价直 next_i: "<<next_i<<" ;costs[next_i]: "<<std::to_string(costs[next_i])<<std::endl;

        int current_x,current_y;//当前索引点的坐标
        // toX_Y(next_i,current_x, current_y, nx_ , ny_);
        current_x = next_i % nx_;
        current_y = next_i / nx_;//计算当前点到终点的曼哈顿距离。  ns_ = nx * ny;
        //计算当前点到终点的曼哈顿距离
        float distance = abs(end_x - current_x) + abs(end_y - current_y);//计算当前点到终点的曼哈顿距离。
        // float distance = manhattanDistanceHeuristicCost(current_x, current_y, end_x, end_y);
        // float distance1 =  (float)TotalDistance(current_x, current_y,  start_x, start_y, end_x, end_y);
        //  float totalCost =distance1+ distance+costs[next_i];//计算当前点的总代价值
        //costmap_->getCharMap() = costs；   /home/sukai/workspace/workspace_ros_car_noetic/src/navigation-noetic/global_planner/include/global_planner/potential_calculator.h  63行
        // potential[next_i] = p_calc_->calculatePotential(potential, costs[next_i] + neutral_cost_, next_i, prev_potential);//计算这个点的potential（预估每个点到目标节点的距离）值。 //potential = float* potential_array_;
        /**
        将已经走过的代价作为当前的代价值的目的是为了考虑到从起点到当前节点的路径上已经经过的代价，这些代价需要在计算当前节点的总代价时进行考虑。这是因为，每个节点的代价不仅与到终点的距离有关，还与该节点所在的路径上的代价有关。
        举个例子，假设一个节点到终点的距离较近，但是该节点所在路径上的代价较高，那么从起点经过该节点到达终点的总代价可能会比从其他路径经过该节点的总代价更高。因此，在计算当前节点的总代价时，需要将已经走过的代价加入计算。
         potential[current_i]：已经走过的代价
         costs[next_i]：当前节点的代价
         如果当前节点的代价值为0，那么当前节点的总代价值就等于已经走过的代价值加上到终点的距离。
         如果 不加上  potential[current_i]：已经走过的代价，那么就搜索不出来路径了；
         float totalCost =distance+costs[next_i]+potential[current_i]+neutral_cost_;//计算当前点的总代价值
         ---
         /home/sukai/workspace/workspace_ros_car_noetic/src/navigation-noetic/global_planner/include/global_planner/potential_calculator.h  63行
         potential[next_i] = p_calc_->calculatePotential(potential, costs[next_i] + neutral_cost_, next_i, prev_potential);//计算这个点的potential（预估每个点到目标节点的距离）值。
        */
        float totalCost =distance+costs[next_i]+potential[current_i]+neutral_cost_;//计算当前点的总代价值
        potential[next_i] = totalCost;//将当前点的总代价值赋值给potential数组
        //  queue_.push_back(Index(next_i, potential[next_i] ));//将这个节点放到搜索队列里，并计算该节点的priority（potential值+距离）。
        queue_.push_back(Index(next_i, potential[next_i],current_i,current_x,current_y ));//将这个节点放到搜索队列里，并计算该节点的priority（potential值+距离）。
        //open_set_.push_back(Index(next_i, potential[next_i],current_i,current_x,current_y ));
        open_set_array_[next_i] = Index(next_i, potential[next_i],current_i,current_x,current_y );
        std::push_heap(queue_.begin(), queue_.end(), greater1());//调整搜索队列，使之满足小根堆的性质，即priority小的节点在队列前面。
        // std::cout<< "add 55555555555555555555555555555555555555 大于代价直 next_i: "<<next_i<<" ;costs[next_i]: "<<std::to_string(costs[next_i])<<std::endl;

    }




    //sukai 从可行点中提取路径，从potential 代价数组中提取路径；//start_x, start_y, goal_x, goal_y 像素坐标；//path 路径点集合
    bool global_planner::getPath(float* potential, int start_x, int start_y, int end_x, int end_y, std::vector<std::pair<int, int> >& path) {
        std::pair<int, int> current;//定义一个pair类型的变量current，用于存储当前点的坐标。
        current.first = end_x;//将终点的x坐标赋值给current的first成员。
        current.second = end_y;//将终点的y坐标赋值给current的second成员。
        ROS_INFO("11111111111111111111getPath1111111111111111111111111111111");
        int start_index = toIndex(start_x, start_y,nx_,ny_);//将起点的坐标转化为数组下标，并赋值给start_index。  start_x, start_y, goal_x, goal_y 像素坐标
        int  end_index = toIndex(end_x, end_y,nx_,ny_);//将起点的坐标转化为数组下标，并赋值给start_index。  start_x, start_y, goal_x, goal_y 像素坐标
        std::cout<<"1. getPath start_index:"<<std::to_string(start_index)<< std::endl;
        std::cout<<"2. getPath end_index:"<<std::to_string(end_index)<< std::endl;

        std::cout<<"3. getPath end_x: "<<std::to_string(end_x)<< std::endl;
        std::cout<<"3. getPath  end_y: "<<std::to_string(end_y)<< std::endl;
//        int length = sizeof(potential) / sizeof(float);
//        std::cout<<"potential-> length: "<<length<< std::endl;
        path.push_back(current);//将终点加入路径中。
        int c = 0;//定义一个计数器c，用于记录路径长度。
        int ns = nx_ * ny_;//计算地图大小。
        std::vector<int> close_set; //已经遍历的节点


        //close_set.push_back(start_index);
        while (toIndex(current.first, current.second,nx_,ny_) != start_index) {// 当当前点不是起点时，执行循环。
           // ROS_INFO("1111113333333333333333333111111getPath111111113333333333333333333333331111111111");
//            if(toIndex(current.first, current.second,nx_,ny_) == start_index){
//                std::cout<<"1 起始点索引 start_index:"<<std::to_string(start_index)<< std::endl;
//                std::cout<<"1 当前索引 toIndex(current.first, current.second,nx_,ny_):"<<std::to_string(toIndex(current.first, current.second,nx_,ny_))<< std::endl;
//                return false;
//            }


            float min_val = 1e10;//定义一个变量min_val，用于存储周围点的最小权值。
            int min_x = 0, min_y = 0;//定义两个变量min_x和min_y，用于存储周围点的坐标。
          //  std::cout<<"----------------------------------------------------------------------"<< std::endl;
            // close_set.push_back(toIndex(current.first, current.second,nx_,ny_));
            for (int xd = -1; xd <= 1; xd++) {//循环遍历周围的点。
                for (int yd = -1; yd <= 1; yd++) {//循环遍历周围的点。
                    if (xd == 0 && yd == 0)//排除当前点。
                    {
                        // close_set.push_back(toIndex(current.first, current.second,nx_,ny_));
                        // potential[toIndex(current.first, current.second,nx_,ny_)]= 1e10;//从待处理的点集合中删除当前点。  赋值为1e10就是删除；
                        continue;
                    }

                    int x = current.first + xd, y = current.second + yd;//计算周围点的坐标。
                    int index = toIndex(x, y,nx_,ny_);//将周围点的坐标转化为数组下标。
//                    if(std::find(close_set.begin(), close_set.end(), index)== close_set.end()){
//                        std::cout<<"7777777777777777777777777777777777777777777777777777"<< std::endl;
//
//                    }

                    if (potential[index] < min_val&& std::find(close_set.begin(), close_set.end(), index)== close_set.end()) {//如果周围点的权值小于min_val。
                     //   std::cout<<"9999999999999999999999999999999999999999999999999999999999"<< std::endl;

                        min_val = potential[index];//将周围点的权值赋值给min_val。
                        min_x = x;//将周围点的x坐标赋值给min_x。
                        min_y = y;//将周围点的y坐标赋值给min_y。
                    }//结束if语句。
                    else{
//                        if(std::find(close_set.begin(), close_set.end(), index)!= close_set.end()){
//                            std::cout<<"/////////+++++++++++++++///////////////2: 已经在列表中了     :"<<std::to_string(index)<< std::endl;
//                            std::cout<<"/////////++++++++++++//////////////2代价 potential[index]:"<<std::to_string(potential[index])<< std::endl;
//                        }
                    }


//                    std::cout<<"1当前索引index+ xd      :"<<std::to_string(index)<< std::endl;
//                    std::cout<<"1当前索引  current.first:"<<std::to_string(toIndex(current.first, current.second,nx_,ny_))<< std::endl;
//                    std::cout<<"1起始点索引 start_index:"<<std::to_string(start_index)<< std::endl;
//                    std::cout<<"1代价 potential[index]             :"<<std::to_string(potential[index])<< std::endl;
//                    std::cout<<"1代价 potential[start_index]             :"<<std::to_string(potential[start_index])<< std::endl;
//                    std::cout<<"1代价 min_val                      :"<<std::to_string(min_val)<< std::endl;
//                    std::cout<<"---"<< std::endl;
//                    std::cout<<"1start_index:"<<std::to_string(start_index)<< std::endl;
//                    std::cout<<"1end_index:"<<std::to_string(end_index)<< std::endl;
//                    std::cout<<"---"<< std::endl;

//                    std::cout<<"-------start-----for-----------------"<< std::endl;
//                    std::cout<<"index:"<<index<< std::endl;
//                    std::cout<<"start_index:"<<start_index<< std::endl;
//                    std::cout<<"x:"<<std::to_string(x)<< std::endl;
//                    std::cout<<"y:"<<std::to_string(y)<< std::endl;
//                    std::cout<<"-------end-------for---------------"<< std::endl;
                }//结束yd的循环。
            }//结束xd的循环
            //   ROS_INFO("1111114444444444444444111111getPath11111111333333444444444444444444444433333331111111111");
            //如果没有找到周围点，返回false。
            if (min_x == 0 && min_y == 0){
//                std::cout<<"zzzzzzzzzzzzzzzzzz起始点索引 start_index:"<<std::to_string(start_index)<< std::endl;
//                std::cout<<"zzzzzzzzzzzzzzzzzz当前索引 t            :"<<std::to_string(toIndex(current.first, current.second,nx_,ny_))<< std::endl;
//
//                ROS_INFO("zzzzzzzzzzzzzzzzzzzzzzzzzzzzzz如果没有找到周围点，返回false。wwwwwwwwwwwwwwwwwwwwwwwwwwwww");
//                for (int i = 0; i <close_set.size() ; ++i) {
//                    std::cout << close_set[i] << " ";
//                    std::cout<<"close_set[i]:"<<close_set[i]<< std::endl;
//                    std::cout<<"close_set.size():"<<std::to_string(close_set.size())<< std::endl;
//                }
                return false;
            }
          //  std::cout<<"--------------while---------------Index: "<< toIndex(current.first, current.second,nx_,ny_)<<std::endl;
            //close_set.push_back(toIndex(current.first, current.second,nx_,ny_));
            current.first = min_x;//将周围点的x坐标赋值给current的first成员。
            current.second = min_y;// 将周围点的y坐标赋值给current的second成员。
            path.push_back(current);//将周围点加入路径中。
            // close_set.push_back(toIndex(current.first, current.second,nx_,ny_));


//            std::cout<<"--------------while---------------"<< std::endl;

//            std::cout<<"qqqqqaqqqqqqaaaaaaaaaaaaaa:"<<std::to_string(c)<< std::endl;
//            std::cout<<"min_x:"<<std::to_string(min_x)<< std::endl;
//            std::cout<<"min_y:"<<std::to_string(min_y)<< std::endl;
//            std::cout<<"current.first:"<<std::to_string(current.first)<< std::endl;
//            std::cout<<"current.first:"<<std::to_string(current.first)<< std::endl;
//            std::cout<<"current.second:"<<std::to_string(current.second)<< std::endl;
//            std::cout<<"current.second:"<<std::to_string(current.second)<< std::endl;
//
//            std::cout<<"起始点索引 start_index:"<<std::to_string(start_index)<< std::endl;
//            std::cout<<"起始点索引 start_index:"<<std::to_string(start_index)<< std::endl;
//            std::cout<<"当前索引 t            :"<<std::to_string(toIndex(current.first, current.second,nx_,ny_))<< std::endl;
//            std::cout<<"当前索引 t            :"<<std::to_string(toIndex(current.first, current.second,nx_,ny_))<< std::endl;
//
//
//            std::cout<<"***************************path.size():"<<path.size()<< std::endl;
//            std::cout<<"***************************ns:"<<ns<< std::endl;
//            std::cout<<"***************************nx_:"<<nx_<< std::endl;
//            std::cout<<"***************************ny_:"<<ny_<< std::endl;
//
            if(c++>ns*4){//如果路径长度超过了地图大小的4倍，返回false。
                //  if(c++>100){//如果路径长度超过了地图大小的4倍，返回false。
               std::cout<<"================ if(c++>ns*4){//如果路径长度超过了地图大小的4倍，返回false。close_set.size(): "<<close_set.size()<< std::endl;
                for (int i = 0; i <close_set.size() ; ++i) {
//                    std::cout << close_set[i] << " ";
//                    std::cout<<"close_set[i]:"<<close_set[i]<< std::endl;
//                    std::cout<<"close_set.size():"<<std::to_string(close_set.size())<< std::endl;
                }
                return false;
            }//结束if语句。

        }// 结束while循环。




        return true;//如果找到了起点，返回true，否则返回false。
    }



    //========================================================

    // algorithm= Dijkstra 算法 ; AStar 算法
    bool  global_planner::makePlanWithSemiStructuredPath2(Point &start_point,Point &goal_point, std::vector<geometry_msgs::PoseStamped> &makePlanWithSemiStructuredPlan,std::vector<Point> &plan_points,string algorithm) {
        std::cout<<"1.makePlanWithSemiStructuredPath2  全局路径规划 algorithm  :"<<algorithm<< std::endl;
        std::cout<<"2:makePlanWithSemiStructuredPath2   全局路径规划  坐标 start_point.x :" <<std::to_string(start_point.x)<<"  : start_point.y:  "<<std::to_string(start_point.y)<< std::endl;
        std::cout<<"3:makePlanWithSemiStructuredPath2   全局路径规划  坐标 goal_point.x :" <<std::to_string(goal_point.x)<<"  : goal_point.y:  "<<std::to_string(goal_point.y)<< std::endl;

        //todo morPlanToMap_startpoint ，target_goal_point这两个点用a* 规划一条路径 【每条路径的终点到每条路径的起点】
        //todo 2.规划路径，半结构路径规划中的 makePlan函数   tolerance =default_tolerance_ //    1451    private_nh2.param("default_tolerance", default_tolerance_, 0.0);//#默认0.0，目标容差
        bool makePlanWithSemiStructuredPathFig = makePlanWithSemiStructuredPath(start_point,
                                                                                goal_point,
                                                                                algorithm,
                                                                                default_tolerance_,
                                                                                makePlanWithSemiStructuredPlan);// Dijkstra 算法 ; AStar 算法

        if (!makePlanWithSemiStructuredPathFig){

            std::cout<<"4:error makePlanWithSemiStructuredPath2   全局路径规划  坐标 start_point.x :" <<std::to_string(start_point.x)<<"  : start_point.y:  "<<std::to_string(start_point.y)<< std::endl;
            std::cout<<"5:error makePlanWithSemiStructuredPath2   全局路径规划  坐标 goal_point.x :" <<std::to_string(goal_point.x)<<"  : goal_point.y:  "<<std::to_string(goal_point.y)<< std::endl;
            return false;
        }




       // std::vector<Point> plan_points;//todo 保存路径 所有点的像素坐标
        for (int j = 0; j < makePlanWithSemiStructuredPlan.size(); ++j) {//把坐标转成像素坐标

            double  wx = makePlanWithSemiStructuredPlan[j].pose.position.x;
            double  wy = makePlanWithSemiStructuredPlan[j].pose.position.y;
            unsigned int current_x_i, current_y_i;

            if (!origin_costmap_->worldToMap(wx, wy, current_x_i, current_y_i)) {
                std::cout<<"6:error makePlanWithSemiStructuredPath2 errror 机器人的起始位置不在全球成本图上。计划总是会失败，你确定机器人已经正确本地化了吗  全局路径规划  坐标 goal_point.x :" <<std::to_string(goal_point.x)<<"  : goal_point.y:  "<<std::to_string(goal_point.y)<< std::endl;

                ROS_WARN_THROTTLE(1.0,
                                  "errror 机器人的起始位置不在全球成本图上。计划总是会失败，你确定机器人已经正确本地化了吗 The goal sent to the global planner is off the global costmap. Planning will always fail to this goal.");
                return false;
            }
            Point point;
            point.x=(double)current_x_i;
            point.y=(double)current_y_i;
            plan_points.push_back(point);

        }

        std::cout<<"7:makePlanWithSemiStructuredPath2   全局路径规划  坐标 goal_point.x :" <<std::to_string(goal_point.x)<<"  : goal_point.y:  "<<std::to_string(goal_point.y)<< std::endl;

        return true;
    }





//====================================================

//=====================================================

    bool global_planner::getPlanFromPotential(float* potential,int start_x, int start_y, int goal_x, int goal_y,
                                                     const geometry_msgs::PoseStamped& goal,
                                                     std::vector<geometry_msgs::PoseStamped>& plan) {
        if (!initialized_) {
            ROS_ERROR(
                    "This planner has not been initialized yet, but it is being used, please call initialize() before use");
            return false;
        }

        std::string global_frame = frame_id_;

        //clear the plan, just in case
        plan.clear();

        std::vector<std::pair<int, int> > path;


        // float* potential_array_;   //potential（预估每个点到目标节点的距离）
        //191  path_maker_ = new GridPath(p_calc_);//new 出path_maker_实例，从可行点中提取路径  ||  path_maker_ = new GradientPath(p_calc_);//梯度下降算法,路径更为光滑点
        if (!getPath(potential, start_x, start_y, goal_x, goal_y, path)) {//start_x, start_y, goal_x, goal_y 像素坐标
            ROS_ERROR("NO PATH!");
            return false;
        }



        ros::Time plan_time = ros::Time::now();
        for (int i = path.size() -1; i>=0; i--) {
            std::pair<float, float> point = path[i];
            //convert the plan to world coordinates
            double world_x, world_y;
            //像素坐标转世界坐标
             mapToWorld(point.first, point.second, world_x, world_y);


            geometry_msgs::PoseStamped pose;
            pose.header.stamp = plan_time;
            pose.header.frame_id = global_frame;
            pose.pose.position.x = world_x;
            pose.pose.position.y = world_y;
            pose.pose.position.z = 0.0;
            pose.pose.orientation.x = 0.0;
            pose.pose.orientation.y = 0.0;
            pose.pose.orientation.z = 0.0;
            pose.pose.orientation.w = 1.0;
            plan.push_back(pose);
        }

        return !plan.empty();
    }



    //计算距离起点的对角距离
    int global_planner::BaseCost(int current_x, int current_y, int start_x,int start_y){
        int x_dis = abs(current_x- start_x);
        int y_dis = abs(current_y - start_y);
        return x_dis + y_dis + (sqrt(2) - 2) * std::min(x_dis, y_dis);
    }
/**
    这段代码是一个名为MyGlobalPlannerPlugin的插件中的函数，函数名为BaseCost。这个函数计算从起点到当前点的代价，代价的计算方式是通过曼哈顿距离和对角线距离的加权和来实现的。
    具体来说，这个函数的参数包括当前点的x坐标和y坐标，以及起点的x坐标和y坐标。函数首先计算当前点到起点的横向距离（x_dis）和纵向距离（y_dis），然后将这两个距离相加，得到曼哈顿距离。
    接下来，函数使用对角线距离来计算当前点到起点的斜向距离，这里使用了一个公式：(sqrt(2) - 2) * std::min(x_dis, y_dis)。其中sqrt(2)是根号2的值，std::min(x_dis, y_dis)表示取x_dis和y_dis中的最小值。
    这个公式的含义是，如果当前点和起点之间的距离是斜向的，那么就用这个公式来计算斜向距离。这个公式的实际作用是计算斜向距离和曼哈顿距离之间的差距，以便更加准确地估算当前点到起点的距离。
    最后，函数将曼哈顿距离和斜向距离加权求和，得到从起点到当前点的代价，并将这个代价作为函数的返回值。
*/
    //计算距离终点的对角距离
    int global_planner::HeuristicCost(int current_x, int current_y,int end_x,int end_y){
        int x_dis = abs(end_x - current_x);
        int  y_dis = abs(end_y - current_y);
        return x_dis + y_dis + (sqrt(2) - 2) * std::min(x_dis, y_dis);
    }
    //总的对角距离
    int global_planner::TotalDistance(int current_x, int current_y, int start_x,int start_y,int end_x,int end_y){
        return BaseCost(current_x, current_y,  start_x, start_y) + HeuristicCost(current_x, current_y, end_x, end_y);
    }


    //计算当前点到终点的曼哈顿距离
    float global_planner::manhattanDistanceHeuristicCost(int &current_x, int &current_y,int end_x,int end_y){
        return   abs(end_x - current_x) + abs(end_y - current_y);//计算当前点到终点的曼哈顿距离。
    }
     //像素坐标转世界坐标
    void global_planner::mapToWorld(double mx, double my, double& wx, double& wy) {
        wx = origin_costmap_->getOriginX() + mx * origin_costmap_->getResolution();
        wy = origin_costmap_->getOriginY() + my * origin_costmap_->getResolution();
    }

/**
 *
 * @param wx
 * @param wy
 * @param mx
 * @param my
 这个函数的实现比较简单，主要包括以下几个步骤：

    检查给定的世界坐标 (wx, wy) 是否在 Costmap2D 对象覆盖的区域内，如果不在，则返回 false。

    将给定的世界坐标 (wx, wy) 转换为相应的离散地图坐标 (mx, my)。

    检查转换后的地图坐标 (mx, my) 是否在 Costmap2D 对象的有效范围内，如果超出，则返回 false。

    如果以上检查都通过了，则返回 true。

下面是对这个函数中各个参数的说明：

    wx 和 wy：待转换的世界坐标。
    mx 和 my：转换后的离散地图坐标。像素坐标
    origin_x_ 和 origin_y_：地图坐标系原点在世界坐标系中的位置。
    resolution_：地图的分辨率，即一个像素代表的实际距离。
    size_x_ 和 size_y_：地图的大小，即像素数。

需要注意的是，以上实现中使用了 C++ 中的类型转换语句 static_cast，将浮点数转换为整数类型，同时使用引用类型 & 将结果返回给调用者。
 */
//世界坐标转像素坐标
    bool global_planner::worldToMap(double wx, double wy, double& mx, double& my) {
        double origin_x = origin_costmap_->getOriginX(), origin_y = origin_costmap_->getOriginY();
        double resolution = origin_costmap_->getResolution();

        if (wx < origin_x || wy < origin_y)
            return false;

        /**
         * //参数：判断使用的是navfn包还是，old_navfn_behavior_这个参数默认为false，用来兼容navfn；然后调用clearEndpoint（）函数，该函数把终点周围的点更新了一下：
        //    old_navfn_behavior(default: false): 若在某些情况下,想让global_planner完全复制navfn的功能,那就设置为true,但是需要注意navfn是非常旧的ROS系统中使用的,现在已经都用global_planner代替navfn了,所以不建议设置为true.

            if(!old_navfn_behavior_)
                convert_offset_ = 0.5;//false
            else
                convert_offset_ = 0.0;//true
        */
        mx = (wx - origin_x) / resolution ;
        my = (wy - origin_y) / resolution ;
        //  costmap_->getSizeInCellsX() 和 getSizeInCellsY：地图的大小，即像素数。
        if (mx < origin_costmap_->getSizeInCellsX() && my < origin_costmap_->getSizeInCellsY())
            return true;

        return false;
    }

    //出于可视化目的发布计划 Path
    void global_planner::publishPlan(const std::vector<geometry_msgs::PoseStamped>& path,nav_msgs::Path &gui_path) {
        if (!initialized_) {
            ROS_ERROR(
                    "未初始化initialized This planner has not been initialized yet, but it is being used, please call initialize() before use");
            return;
        }

        //create a message for the plan
        //nav_msgs::Path gui_path;
        gui_path.poses.resize(path.size());

        gui_path.header.frame_id = frame_id_;
        gui_path.header.stamp = ros::Time::now();

        // Extract the plan in world co-ordinates, we assume the path is all in the same frame
        for (unsigned int i = 0; i < path.size(); i++) {
            gui_path.poses[i] = path[i];
        }

      //  plan_pub_.publish(gui_path);
    }


    //出于可视化目的发布计划 MarkerPlan, int rgb 颜色 1：红，2：绿，3 蓝， ,doubl scale  0.1大小
    void global_planner::publishMarkerPlan(visualization_msgs::Marker &marker,const std::vector<geometry_msgs::PoseStamped>& path,int rgb,double scale) {

        if (!initialized_) {
            ROS_ERROR(
                    "未初始化initialized This planner has not been initialized yet, but it is being used, please call initialize() before use");
            return;
        }

        uint32_t shape =visualization_msgs::Marker::LINE_STRIP ;//ARROW 箭头 ，CUBE 立方体序列,LINE_STRIP 线条（点的连线） LINE_LIST 线条序列 ,POINTS点序列
        //create a message for the plan
        //nav_msgs::Path gui_path;

        marker.header.frame_id = frame_id_;
        marker.header.stamp = ros::Time::now();
        marker.ns = "publishMarkerPlan";//以后改成当前地图名称 ，命名空间 用当前地图名称+场景num，从数据库读取

        // %Tag(ACTION)%
        marker.action = visualization_msgs::Marker::ADD;


        // Extract the plan in world co-ordinates, we assume the path is all in the same frame
        for (unsigned int i = 0; i < path.size(); i++) {



            marker.type = shape; //visualization_msgs::Marker::CUBE 6 立方体序列
            // %Tag(TYPE)%
           // marker.type = shape; //visualization_msgs::Marker::CUBE 6 立方体序列
            //setMarkerType(shape);
            marker.points.push_back(path[i].pose.position);
          //  cout << "publishMarkerPlan pose.position.x:path[i].pose.position.y " << to_string(path[i].pose.position.x) << ", " << to_string(path[i].pose.position.y)  << endl;

        }
        marker.ns = "publishMarkerPlan";//以后改成当前地图名称 ，命名空间 用当前地图名称+场景num，从数据库读取
        //marker.lifetime = ros::Duration(1);
        marker.id = markerId++;
        marker.pose.position.x = 0;
        marker.pose.position.y = 0;
        marker.pose.position.z = 0;
        marker.pose.orientation.x = 0.0;
        marker.pose.orientation.y = 0.0;
        marker.pose.orientation.z = 0.0;
        marker.pose.orientation.w = 1.0;
        if(rgb==1)
        {
            marker.color.r = 1.0f;
            marker.color.g = 0.0f;
            marker.color.b = 0.0f;
        }
        else if(rgb==2)
        {
            marker.color.r = 0.0f;
            marker.color.g = 1.0f;
            marker.color.b = 0.0f;
        }
        else if(rgb==3)
        {
            marker.color.r = 0.0f;
            marker.color.g = 0.0f;
            marker.color.b = 1.0f;
        }
        else
        {
            marker.color.r = 1.0f;
            marker.color.g = 1.0f;
            marker.color.b = 1.0f;
        }

        marker.color.a = 1.0;

        // %Tag(SCALE)%
        if(scale==0)
        {
            marker.scale.x = 0.1;
            marker.scale.y = 0.1;
            marker.scale.z = 0.1;
        }
        else
        {
            marker.scale.x = scale;
            marker.scale.y = scale;
            marker.scale.z = scale;
        }

       // marker_pub_.publish(marker);

    }
    //设置marker类型
    void global_planner::setMarkerType(uint32_t &shape) {
        switch (shape)
        {
            case visualization_msgs::Marker::CUBE://立方体序列

                shape = visualization_msgs::Marker::SPHERE;//球序列
                break;
            case visualization_msgs::Marker::SPHERE://球序列
                shape = visualization_msgs::Marker::ARROW;//箭头
                break;
            case visualization_msgs::Marker::ARROW://箭头
                shape = visualization_msgs::Marker::CYLINDER;//圆柱体
                break;
            case visualization_msgs::Marker::CYLINDER://圆柱体
                shape = visualization_msgs::Marker::CUBE;//立方体序列
                break;
        }


    }

    //计算路径的朝向，计算向量的斜率，向量的朝向,夹角，角度
    tf2::Quaternion global_planner::getSlopeOfVector(double sart_x, double sart_y,double end_x, double end_y,double &yaw){
        double x=end_x-sart_x;
        double y=end_y-sart_y;
        yaw= atan2(y,x);
        //欧拉角转4元素
        tf2::Vector3 axis(0, 0, 1);
        tf2::Quaternion q(axis, yaw);

//        geometry_msgs::PoseStamped pose_stamped;
//        pose_stamped.pose.orientation.x = q.x();
//        pose_stamped.pose.orientation.y = q.y();
//        pose_stamped.pose.orientation.z = q.z();
//        pose_stamped.pose.orientation.w = q.w();

        return q;
    }

    /**
 * 这个函数是一个全局路径规划器的类中的一个函数，用于发布潜在代价地图。函数的输入是一个浮点型数组，表示每个点到目标节点的距离。函数的输出是一个nav_msgs::OccupancyGrid类型的消息，表示整个代价地图。
 * 函数首先获取代价地图的大小和分辨率，
 * 然后将整个代价地图发布出去。在发布代价地图之前，函数会对代价地图进行处理，将代价地图中的每个点的值进行归一化处理，使得代价地图中的最大值为1，最小值为0。最后，函数将处理后的代价地图发布出去。
 * @param potential
 */
    void global_planner::publishPotential(float* potential)
    {
        int nx = origin_costmap_->getSizeInCellsX(), ny = origin_costmap_->getSizeInCellsY();
        double resolution = origin_costmap_->getResolution();
        nav_msgs::OccupancyGrid grid;
        // Publish Whole Grid
        grid.header.frame_id = frame_id_;
        grid.header.stamp = ros::Time::now();
        grid.info.resolution = resolution;

        grid.info.width = nx;
        grid.info.height = ny;

        double wx, wy;
        //世界坐标转像素坐标
        origin_costmap_->mapToWorld(0, 0, wx, wy);
        grid.info.origin.position.x = wx - resolution / 2;
        grid.info.origin.position.y = wy - resolution / 2;
        grid.info.origin.position.z = 0.0;
        grid.info.origin.orientation.w = 1.0;

        grid.data.resize(nx * ny);

        float max = 0.0;
        for (unsigned int i = 0; i < grid.data.size(); i++) {
            float potential_i = potential[i];
            if (potential_i < POT_HIGH) {//potential（预估每个点到目标节点的距离） 值
                if (potential_i > max) {
                    max = potential_i;
                }
            }
        }

        for (unsigned int i = 0; i < grid.data.size(); i++) {
            if (potential[i] >= POT_HIGH) {
                grid.data[i] = -1;
            } else {
                if (fabs(max) < DBL_EPSILON) {
                    grid.data[i] = -1;
                } else {
                    grid.data[i] = potential[i] * publish_scale_ / max;
                }
            }
        }
        // //发布代价地图
        potential_pub_.publish(grid);
    }




//-------------------------------------------------------


//start=================路径优化===========================
/**
// Helper function to compute the euclidean distance between two points 计算两点之间欧氏距离的辅助函数
    double  global_planner::euclidean_distance(const geometry_msgs::PoseStamped& p1, const geometry_msgs::PoseStamped& p2) {
        double dx = p1.pose.position.x - p2.pose.position.x;
        double dy = p1.pose.position.y - p2.pose.position.y;
        return std::sqrt(dx * dx + dy * dy);
    }

// Function to remove redundant points in the path 删除路径中多余点的功能
    void  global_planner::sparsify_path(std::vector<geometry_msgs::PoseStamped>& path, double distance_threshold) {
    if (path.size() < 3) {
    return;
     }

    std::vector<geometry_msgs::PoseStamped> sparse_path;
    sparse_path.push_back(path.front());

    for (std::size_t i = 1; i < path.size() - 1; ++i) {
    double dist_to_prev = euclidean_distance(path[i], sparse_path.back());
    if (dist_to_prev > distance_threshold) {
    sparse_path.push_back(path[i]);
    }
    }

    sparse_path.push_back(path.back());
    path.swap(sparse_path);
}

// Function to smooth the path using a simple moving average 使用简单移动平均值平滑路径的函数
void  global_planner::smooth_path(std::vector<geometry_msgs::PoseStamped>& path, int window_size) {
        if (path.size() <= window_size) {
            return;
        }

        std::vector<geometry_msgs::PoseStamped> smooth_path(path.size());
        for (std::size_t i = 0; i < path.size(); ++i) {
            int count = 0;
            double x_sum = 0, y_sum = 0;
            double q0_sum = 0, q1_sum = 0, q2_sum = 0, q3_sum = 0;

            for (int j = -window_size / 2; j <= window_size / 2; ++j) {
                int idx = i + j;
                if (idx >= 0 && idx < path.size()) {
                    x_sum += path[idx].pose.position.x;
                    y_sum += path[idx].pose.position.y;
                    q0_sum += path[idx].pose.orientation.w;
                    q1_sum += path[idx].pose.orientation.x;
                    q2_sum += path[idx].pose.orientation.y;
                    q3_sum += path[idx].pose.orientation.z;
                    count++;
                }
            }

            smooth_path[i].pose.position.x = x_sum / count;
            smooth_path[i].pose.position.y = y_sum / count;
            smooth_path[i].pose.orientation.w = q0_sum / count;
            smooth_path[i].pose.orientation.x = q1_sum / count;
            smooth_path[i].pose.orientation.y = q2_sum / count;
            smooth_path[i].pose.orientation.z = q3_sum / count;

            // Normalize the quaternion to avoid invalid values
            double quat_norm = std::sqrt(smooth_path[i].pose.orientation.w * smooth_path[i].pose.orientation.w +
                                         smooth_path[i].pose.orientation.x * smooth_path[i].pose.orientation.x +
                                         smooth_path[i].pose.orientation.y * smooth_path[i].pose.orientation.y +
                                         smooth_path[i].pose.orientation.z * smooth_path[i].pose.orientation.z);
            smooth_path[i].pose.orientation.w /= quat_norm;
            smooth_path[i].pose.orientation.x /= quat_norm;
            smooth_path[i].pose.orientation.y /= quat_norm;
            smooth_path[i].pose.orientation.z /= quat_norm;
        }

        path.swap(smooth_path);
}

// Function to optimize the global path 用于优化全局路径的功能
void  global_planner::optimize_global_path(std::vector<geometry_msgs::PoseStamped>& path, double distance_threshold, int window_size) {
sparsify_path(path, distance_threshold);
smooth_path(path, window_size);
// Adjust the length of the path and ensure connectivity between start and goal points if needed 如果需要，调整路径长度并确保起点和终点之间的连接
}
*/
//============================2================================================
//欧氏距离
    double global_planner::euclidean_distance(const geometry_msgs::Point& p1, const geometry_msgs::Point& p2) {
        double dx = p1.x - p2.x;
        double dy = p1.y - p2.y;
        return std::sqrt(dx * dx + dy * dy);
    }
//插入中间点
    void global_planner::insert_intermediate_points(std::vector<geometry_msgs::PoseStamped>& path, double max_distance) {
        std::vector<geometry_msgs::PoseStamped> new_path;
        new_path.reserve(path.size() * 2);

        for (size_t i = 0; i < path.size() - 1; ++i) {
            new_path.push_back(path[i]);

            double dist = euclidean_distance(path[i].pose.position, path[i + 1].pose.position);
            int num_insertions = static_cast<int>(std::floor(dist / max_distance)) - 1;

            for (int j = 0; j < num_insertions; ++j) {
                double ratio = static_cast<double>(j + 1) / (num_insertions + 1);
                geometry_msgs::PoseStamped interpolated_pose;
                interpolated_pose.pose.position.x = path[i].pose.position.x + ratio * (path[i + 1].pose.position.x - path[i].pose.position.x);
                interpolated_pose.pose.position.y = path[i].pose.position.y + ratio * (path[i + 1].pose.position.y - path[i].pose.position.y);
                new_path.push_back(interpolated_pose);
            }
        }

        new_path.push_back(path.back());
        path.swap(new_path);
    }
///平滑路径
    void global_planner::smooth_path(std::vector<geometry_msgs::PoseStamped>& path, int window_size) {
        if (path.size() <= window_size) {
            return;
        }

        std::vector<geometry_msgs::PoseStamped> smooth_path(path.size());
        for (std::size_t i = 0; i < path.size(); ++i) {
            int count = 0;
            double x_sum = 0, y_sum = 0;

            for (int j = -window_size / 2; j <= window_size / 2; ++j) {
                int idx = i + j;
                if (idx >= 0 && idx < path.size()) {
                    x_sum += path[idx].pose.position.x;
                    y_sum += path[idx].pose.position.y;
                    count++;
                }
            }

            smooth_path[i].pose.position.x = x_sum / count;
            smooth_path[i].pose.position.y = y_sum / count;
        }

        path.swap(smooth_path);
    }
//计算方向
    void global_planner::calculate_orientations(std::vector<geometry_msgs::PoseStamped>& path) {
        for (size_t i = 0; i < path.size() - 1; ++i) {
            double dx = path[i + 1].pose.position.x - path[i].pose.position.x;
            double dy = path[i + 1].pose.position.y - path[i].pose.position.y;
            double yaw = std::atan2(dy, dx);

            path[i].pose.orientation.x = 0;
            path[i].pose.orientation.y = 0;
            path[i].pose.orientation.z = sin(yaw * 0.5);
            path[i].pose.orientation.w = cos(yaw * 0.5);
        }

        path.back().pose.orientation = path[path.size() - 2].pose.orientation;
    }

//end===============路径优化=====================



    //--namespace--
}

//=======================main================================

global_planner *currency = NULL;

//ctrl + c
void mySigintHandler(int sig) {
    std::cout << "ctrl c  sig: " << sig << std::endl;
    if (currency != NULL)
        delete currency;

    ros::shutdown();
}

// rosrun contnav_global_planner contnav_global_planner_node
int main(int argc, char **argv) {
    //main中init前面加
    setlocale(LC_ALL, "");  //中文
    ros::init(argc, argv, "contnav_global_planner_node");
    ros::NodeHandle nh;
    signal(SIGINT, mySigintHandler);
    tf2_ros::Buffer buffer(ros::Duration(30));
    tf2_ros::TransformListener tf(buffer);
    currency = new global_planner(buffer,nh);
    currency->run();

    return 0;
}

